internal static global::System.Runtime.InteropServices.HandleRef getCPtr(b2Vec2 obj)
{
return((obj == null) ? new global::System.Runtime.InteropServices.HandleRef(null, global::System.IntPtr.Zero) : obj.swigCPtr);
}
public override void SolveVelocityConstraints(b2TimeStep step)
{
//B2_NOT_USED(step);
b2Mat22 tMat;
float tX;
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
b2Vec2 vA = bA.m_linearVelocity;
float wA = bA.m_angularVelocity;
b2Vec2 vB = bB.m_linearVelocity;
float wB = bB.m_angularVelocity;
float mA = bA.m_invMass;
float mB = bB.m_invMass;
float iA = bA.m_invI;
float iB = bB.m_invI;
//b2Vec2 rA = b2Mul(bA->m_xf.R, m_localAnchorA - bA->GetLocalCenter());
tMat = bA.m_xf.R;
float rAX = m_localAnchorA.x - bA.m_sweep.localCenter.x;
float rAY = m_localAnchorA.y - bA.m_sweep.localCenter.y;
tX = (tMat.col1.x * rAX + tMat.col2.x * rAY);
rAY = (tMat.col1.y * rAX + tMat.col2.y * rAY);
rAX = tX;
//b2Vec2 rB = b2Mul(bB->m_xf.R, m_localAnchorB - bB->GetLocalCenter());
tMat = bB.m_xf.R;
float rBX = m_localAnchorB.x - bB.m_sweep.localCenter.x;
float rBY = m_localAnchorB.y - bB.m_sweep.localCenter.y;
tX = (tMat.col1.x * rBX + tMat.col2.x * rBY);
rBY = (tMat.col1.y * rBX + tMat.col2.y * rBY);
rBX = tX;
// Solve point-to-point constraint
float Cdot1X = vB.x - wB * rBY - vA.x + wA * rAY;
float Cdot1Y = vB.y + wB * rBX - vA.y - wA * rAX;
float Cdot2 = wB - wA;
b2Vec3 impulse = new b2Vec3();
m_mass.Solve33(impulse, -Cdot1X, -Cdot1Y, -Cdot2);
m_impulse.Add(impulse);
vA.x -= mA * impulse.x;
vA.y -= mA * impulse.y;
wA -= iA * (rAX * impulse.y - rAY * impulse.x + impulse.z);
vB.x += mB * impulse.x;
vB.y += mB * impulse.y;
wB += iB * (rBX * impulse.y - rBY * impulse.x + impulse.z);
// References has made some sets unnecessary
//bA->m_linearVelocity = vA;
bA.m_angularVelocity = wA;
//bB->m_linearVelocity = vB;
bB.m_angularVelocity = wB;
}
public override void SolveVelocityConstraints(b2SolverData data)
{
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
b2Vec2 vA = data.velocities[m_indexA].v;
float wA = data.velocities[m_indexA].w;
b2Vec2 vB = data.velocities[m_indexB].v;
float wB = data.velocities[m_indexB].w;
// Solve spring raint
{
float Cdot = b2Math.b2Dot(m_ax, vB - vA) + m_sBx * wB - m_sAx * wA;
float impulse = -m_springMass * (Cdot + m_bias + m_gamma * m_springImpulse);
m_springImpulse += impulse;
b2Vec2 P = impulse * m_ax;
float LA = impulse * m_sAx;
float LB = impulse * m_sBx;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
// Solve rotational motor raint
{
float Cdot = wB - wA - m_motorSpeed;
float impulse = -m_motorMass * Cdot;
float oldImpulse = m_motorImpulse;
float maxImpulse = data.step.dt * m_maxMotorTorque;
m_motorImpulse = b2Math.b2Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
wA -= iA * impulse;
wB += iB * impulse;
}
// Solve point to line raint
{
float Cdot = b2Math.b2Dot(m_ay, vB - vA) + m_sBy * wB - m_sAy * wA;
float impulse = -m_mass * Cdot;
m_impulse += impulse;
b2Vec2 P = impulse * m_ay;
float LA = impulse * m_sAy;
float LB = impulse * m_sBy;
vA -= mA * P;
wA -= iA * LA;
vB += mB * P;
wB += iB * LB;
}
data.velocities[m_indexA].v = vA;
data.velocities[m_indexA].w = wA;
data.velocities[m_indexB].v = vB;
data.velocities[m_indexB].w = wB;
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input, ref b2Transform xf, int childIndex)
{
output = b2RayCastOutput.Zero;
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = Vertex1;
b2Vec2 v2 = Vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 normal = b2Vec2.Zero; // new b2Vec2(e.y, -e.x);
normal.x = e.y;
normal.y = -e.x;
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
b2Vec2 diff = v1 - p1;
float numerator = b2Math.b2Dot(ref normal, ref diff);
float denominator = b2Math.b2Dot(ref normal, ref d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return(false);
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float rr = r.LengthSquared; // b2Math.b2Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
diff = q - v1;
float s = b2Math.b2Dot(ref diff, ref r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -normal;
}
else
{
output.normal = normal;
}
return(true);
}
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(b2Manifold manifold, b2PolygonShape polygonA, ref b2Transform xfA, b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c;
c.x = (xfB.q.c * circleB.Position.x - xfB.q.s * circleB.Position.y) + xfB.p.x;
c.y = (xfB.q.s * circleB.Position.x + xfB.q.c * circleB.Position.y) + xfB.p.y;
b2Vec2 cLocal;
float px = c.x - xfA.p.x;
float py = c.y - xfA.p.y;
cLocal.x = (xfA.q.c * px + xfA.q.s * py);
cLocal.y = (-xfA.q.s * px + xfA.q.c * py);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.m_vertexCount;
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
b2Vec2 tmp;
tmp.x = cLocal.x - vertices[i].x;
tmp.y = cLocal.y - vertices[i].y;
float s = normals[i].x * tmp.x + normals[i].y * tmp.y; // b2Math.b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint.x = 0.5f * (v1.x + v2.x);
manifold.localPoint.y = 0.5f * (v1.y + v2.y);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float ax = cLocal.x - v1.x;
float ay = cLocal.y - v1.y;
float bx = v2.x - v1.x;
float by = v2.y - v1.y;
float u1 = ax * bx + ay * by;
ax = cLocal.x - v2.x;
ay = cLocal.y - v2.y;
bx = v1.x - v2.x;
by = v1.y - v2.y;
float u2 = ax * bx + ay * by;
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(ref cLocal, ref v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(ref cLocal, ref v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal.x = cLocal.x - v2.x;
manifold.localNormal.y = cLocal.y - v2.y;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
else
{
b2Vec2 faceCenter;
faceCenter.x = 0.5f * (v1.x + v2.x);
faceCenter.y = 0.5f * (v1.y + v2.y);
b2Vec2 a;
a.x = cLocal.x - faceCenter.x;
a.y = cLocal.y - faceCenter.y;
separation = b2Math.b2Dot(ref a, ref normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id.key = 0;
}
}
/**
* Test a point for containment in this fixture.
* @param xf the shape world transform.
* @param p a point in world coordinates.
*/
public bool TestPoint(b2Vec2 p)
{
return(m_shape.TestPoint(m_body.GetTransform(), p));
}
// TODO_ERIN might not need to return the separation
public float Initialize(ref b2SimplexCache cache,
b2DistanceProxy proxyA, ref b2Sweep sweepA,
b2DistanceProxy proxyB, ref b2Sweep sweepB,
float t1, ref b2Transform xfA, ref b2Transform xfB)
{
m_proxyA = proxyA;
m_proxyB = proxyB;
int count = cache.count;
Debug.Assert(0 < count && count < 3);
m_sweepA = sweepA;
m_sweepB = sweepB;
if (count == 1)
{
m_type = SeparationType.e_points;
b2Vec2 localPointA = m_proxyA.m_vertices[(int)cache.indexA[0]];
b2Vec2 localPointB = m_proxyB.m_vertices[(int)cache.indexB[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
m_axis.x = pointBx - pointAx;
m_axis.y = pointBy - pointAy;
float s = m_axis.Normalize();
return(s);
}
else if (cache.indexA[0] == cache.indexA[1])
{
// Two points on B and one on A.
m_type = SeparationType.e_faceB;
b2Vec2 localPointB1 = proxyB.m_vertices[(int)cache.indexB[0]];
b2Vec2 localPointB2 = proxyB.m_vertices[(int)cache.indexB[1]];
float b21x = localPointB2.x - localPointB1.x;
float b21y = localPointB2.y - localPointB1.y;
m_axis.x = -b21y;
m_axis.y = b21x;
// m_axis = b2Math.b2Cross(localPointB2 - localPointB1, 1.0f);
m_axis.Normalize();
float normalx = xfB.q.c * m_axis.x - xfB.q.s * m_axis.y;
float normaly = xfB.q.s * m_axis.x + xfB.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointB1.x + localPointB2.x);
m_localPoint.y = 0.5f * (localPointB1.y + localPointB2.y);
float pointBx = (xfB.q.c * m_localPoint.x - xfB.q.s * m_localPoint.y) + xfB.p.x;
float pointBy = (xfB.q.s * m_localPoint.x + xfB.q.c * m_localPoint.y) + xfB.p.y;
b2Vec2 localPointA = proxyA.m_vertices[(int)cache.indexA[0]];
float pointAx = (xfA.q.c * localPointA.x - xfA.q.s * localPointA.y) + xfA.p.x;
float pointAy = (xfA.q.s * localPointA.x + xfA.q.c * localPointA.y) + xfA.p.y;
float aminusbx = pointAx - pointBx;
float aminusby = pointAy - pointBy;
float s = aminusbx * normalx + aminusby * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
s = -s;
}
return(s);
}
else
{
// Two points on A and one or two points on B.
m_type = SeparationType.e_faceA;
b2Vec2 localPointA1 = m_proxyA.m_vertices[cache.indexA[0]];
b2Vec2 localPointA2 = m_proxyA.m_vertices[cache.indexA[1]];
float a2minusa1x = localPointA2.x - localPointA1.x;
float a2minusa1y = localPointA2.y - localPointA1.y;
//m_axis = a2minusa1.UnitCross();// b2Math.b2Cross(localPointA2 - localPointA1, 1.0f);
m_axis.x = a2minusa1y;
m_axis.y = -a2minusa1x;
m_axis.Normalize();
float normalx = xfA.q.c * m_axis.x - xfA.q.s * m_axis.y;
float normaly = xfA.q.s * m_axis.x + xfA.q.c * m_axis.y;
m_localPoint.x = 0.5f * (localPointA1.x + localPointA2.x);
m_localPoint.y = 0.5f * (localPointA1.y + localPointA2.y);
float pointAx = (xfA.q.c * m_localPoint.x - xfA.q.s * m_localPoint.y) + xfA.p.x;
float pointAy = (xfA.q.s * m_localPoint.x + xfA.q.c * m_localPoint.y) + xfA.p.y;
b2Vec2 localPointB = m_proxyB.m_vertices[cache.indexB[0]];
float pointBx = (xfB.q.c * localPointB.x - xfB.q.s * localPointB.y) + xfB.p.x;
float pointBy = (xfB.q.s * localPointB.x + xfB.q.c * localPointB.y) + xfB.p.y;
float bminusax = pointBx - pointAx;
float bminusay = pointBy - pointAy;
float s = bminusax * normalx + bminusay * normaly;
if (s < 0.0f)
{
m_axis.x = -m_axis.x;
m_axis.y = -m_axis.y;
s = -s;
}
return(s);
}
}
public override void DrawSolidCircle(b2Vec2 center, float radius, b2Vec2 axis, b2Color color)
{
const float k_segments = 16.0f;
const float k_increment = (float)(2.0f * System.Math.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)
{
b2Vec2 v = center + radius * new b2Vec2((float)System.Math.Cos(theta), (float)System.Math.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)
{
b2Vec2 v = center + radius * new b2Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
Gl.glVertex2f(v.x, v.y);
theta += k_increment;
}
Gl.glEnd();
b2Vec2 p = center + radius * axis;
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex2f(center.x, center.y);
Gl.glVertex2f(p.x, p.y);
Gl.glEnd();
}
public static CCVector2 ToCCVector2(this b2Vec2 vec)
{
return(new CCVector2(vec.x, vec.y));
}
public static CCPoint ToCCPoint(this b2Vec2 vec)
{
return(new CCPoint(vec.x, vec.y));
}
public override void DrawSegment(b2Vec2 p1, b2Vec2 p2, b2Color color)
{
DrawNode.AddLineVertex(p1.ToVectorC4B(color.ToCCColor4B(), PTMRatio));
DrawNode.AddLineVertex(p2.ToVectorC4B(color.ToCCColor4B(), PTMRatio));
}
public static void b2FindIncidentEdge(b2ClipVertex[] c,
b2PolygonShape poly1, ref b2Transform xf1, int edge1,
b2PolygonShape poly2, ref b2Transform xf2)
{
#if false
b2Vec2[] normals1 = poly1.Normals;
int count2 = poly2.VertexCount;
b2Vec2[] vertices2 = poly2.Vertices;
b2Vec2[] normals2 = poly2.Normals;
// Get the normal of the reference edge in poly2's frame.
b2Vec2 normal1 = b2Math.b2MulT(xf2.q, b2Math.b2Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = b2Math.b2Dot(ref normal1, ref normals2[i]);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
c[0].v = b2Math.b2Mul(ref xf2, ref vertices2[i1]);
c[0].id.indexA = (byte)edge1;
c[0].id.indexB = (byte)i1;
c[0].id.typeA = b2ContactFeatureType.e_face;
c[0].id.typeB = b2ContactFeatureType.e_vertex;
c[1].v = b2Math.b2Mul(ref xf2, ref vertices2[i2]);
c[1].id.indexA = (byte)edge1;
c[1].id.indexB = (byte)i2;
c[1].id.typeA = b2ContactFeatureType.e_face;
c[1].id.typeB = b2ContactFeatureType.e_vertex;
#else
var edge = poly1.Normals[edge1];
int count2 = poly2.VertexCount;
b2Vec2[] vertices2 = poly2.Vertices;
b2Vec2[] normals2 = poly2.Normals;
// Get the normal of the reference edge in poly2's frame.
float bx = xf1.q.c * edge.x - xf1.q.s * edge.y;
float by = xf1.q.s * edge.x + xf1.q.c * edge.y;
float normal1x = xf2.q.c * bx + xf2.q.s * by;
float normal1y = -xf2.q.s * bx + xf2.q.c * by;
// Find the incident edge on poly2.
int index = 0;
float minDot = b2Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
var normal = normals2[i];
float dot = normal1x * normal.x + normal1y * normal.y;
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
// Build the clip vertices for the incident edge.
int i1 = index;
int i2 = i1 + 1 < count2 ? i1 + 1 : 0;
b2ClipVertex vertex;
var vi1 = vertices2[i1];
var vi2 = vertices2[i2];
vertex.v.x = (xf2.q.c * vi1.x - xf2.q.s * vi1.y) + xf2.p.x;
vertex.v.y = (xf2.q.s * vi1.x + xf2.q.c * vi1.y) + xf2.p.y;
vertex.id.indexA = (byte)edge1;
vertex.id.indexB = (byte)i1;
vertex.id.typeA = b2ContactFeatureType.e_face;
vertex.id.typeB = b2ContactFeatureType.e_vertex;
c[0] = vertex;
vertex.v.x = (xf2.q.c * vi2.x - xf2.q.s * vi2.y) + xf2.p.x;
vertex.v.y = (xf2.q.s * vi2.x + xf2.q.c * vi2.y) + xf2.p.y;
vertex.id.indexA = (byte)edge1;
vertex.id.indexB = (byte)i2;
vertex.id.typeA = b2ContactFeatureType.e_face;
vertex.id.typeB = b2ContactFeatureType.e_vertex;
c[1] = vertex;
#endif
}
public static void b2CollidePolygons(b2Manifold manifold, b2PolygonShape polyA, ref b2Transform xfA, b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.type = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = _incidentEdge;
b2FindIncidentEdge(incidentEdge, poly1, ref xf1, edge1, poly2, ref xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent;
localTangent.x = v12.x - v11.x;
localTangent.y = v12.y - v11.y;
localTangent.Normalize();
b2Vec2 localNormal;
localNormal.x = localTangent.y; //.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f);
localNormal.y = -localTangent.x;
b2Vec2 planePoint;
planePoint.x = 0.5f * (v11.x + v12.x);
planePoint.y = 0.5f * (v11.y + v12.y);
b2Vec2 tangent;
tangent.x = xf1.q.c * localTangent.x - xf1.q.s * localTangent.y;
tangent.y = xf1.q.s * localTangent.x + xf1.q.c * localTangent.y;
float normalx = tangent.y; //UnitCross(); // b2Math.b2Cross(tangent, 1.0f);
float normaly = -tangent.x;
float v11x = (xf1.q.c * v11.x - xf1.q.s * v11.y) + xf1.p.x;
float v11y = (xf1.q.s * v11.x + xf1.q.c * v11.y) + xf1.p.y;
float v12x = (xf1.q.c * v12.x - xf1.q.s * v12.y) + xf1.p.x;
float v12y = (xf1.q.s * v12.x + xf1.q.c * v12.y) + xf1.p.y;
// Face offset.
float frontOffset = normalx * v11x + normaly * v11y;
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -(tangent.x * v11x + tangent.y * v11y) + totalRadius;
float sideOffset2 = tangent.x * v12x + tangent.y * v12y + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = _clipPoints1;
int np;
// Clip to box side 1
b2Vec2 t;
t.x = -tangent.x;
t.y = -tangent.y;
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, ref t, sideOffset1, (byte)iv1);
if (np < 2)
{
return;
}
b2ClipVertex[] clipPoints2 = _clipPoints2;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, ref tangent, sideOffset2, (byte)iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
var v = clipPoints2[i].v;
//float separation = b2Math.b2Dot(ref normal, ref v) - frontOffset;
float separation = normalx * v.x + normaly * v.y - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
//cp.localPoint = b2Math.b2MulT(ref xf2, ref v);
float px = v.x - xf2.p.x;
float py = v.y - xf2.p.y;
cp.localPoint.x = (xf2.q.c * px + xf2.q.s * py);
cp.localPoint.y = (-xf2.q.s * px + xf2.q.c * py);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public override void DrawCircle(b2Vec2 center, float radius, b2Color color)
{
const float k_segments = 16.0f;
const float k_increment = (float)(2.0f * System.Math.PI / k_segments);
float theta = 0.0f;
Gl.glColor3f(color.r, color.g, color.b);
Gl.glBegin(Gl.GL_LINE_LOOP);
for (int i = 0; i < k_segments; ++i)
{
b2Vec2 v = center + radius * new b2Vec2((float)System.Math.Cos(theta), (float)System.Math.Sin(theta));
Gl.glVertex2f(v.x, v.y);
theta += k_increment;
}
Gl.glEnd();
}
public override b2Vec2 GetAnchorB()
{
b2Vec2 ret = new b2Vec2(Box2DPINVOKE.b2GearJoint_GetAnchorB(swigCPtr), true);
return(ret);
}
public override void DrawPolygon(b2Vec2[] vertices, int vertexCount, b2Color 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();
}
public override b2Vec2 GetReactionForce(float inv_dt)
{
b2Vec2 ret = new b2Vec2(Box2DPINVOKE.b2GearJoint_GetReactionForce(swigCPtr, inv_dt), true);
return(ret);
}
private void Form1_Load(object sender, EventArgs e)
{
sogc = new SimpleOpenGlControl();
sogc.Dock = DockStyle.Fill;
sogc.Paint += new PaintEventHandler(sogc_Paint);
sogc.Resize += new EventHandler(sogc_Resize);
Controls.Add(sogc);
sogc.InitializeContexts();
InitOpenGL(sogc.Size, 1, PointF.Empty);
// Define the gravity vector.
b2Vec2 gravity = new b2Vec2(0.0f, -10.0f);
// Do we want to let bodies sleep?
bool doSleep = true;
// Construct a world object, which will hold and simulate the rigid bodies.
world = new b2World(gravity, doSleep);
// world.SetWarmStarting(true);
{
b2BodyDef bd = new b2BodyDef();
b2Body ground = world.CreateBody(bd);
b2PolygonShape shape = new b2PolygonShape();
shape.SetAsEdge(new b2Vec2(-40.0f, 0.0f), new b2Vec2(40.0f, 0.0f));
ground.CreateFixture(shape, 0.0f);
}
{
float a = 0.5f;
b2CircleShape shape = new b2CircleShape();
shape.m_radius = a;
b2Vec2 x = new b2Vec2(-7.0f, 0.95f);
b2Vec2 y;
b2Vec2 deltaX = new b2Vec2(0, 1.25f);
b2Vec2 deltaY = new b2Vec2(0, 1.25f);
y= deltaY;
for (int j = 0; j < 8; ++j)
{
b2BodyDef bd = new b2BodyDef();
bd.type = b2BodyType.b2_dynamicBody;
bd.position = y;
b2Body body = world.CreateBody(bd);
body.CreateFixture(shape, 5.0f);
y += deltaY;
}
}
GDIDebugThing.instance.SetFlags(EDebugFlags.e_shapeBit);
world.SetDebugDraw(GDIDebugThing.instance);
System.Timers.Timer timer = new System.Timers.Timer();
timer.Interval = 85;
timer.SynchronizingObject = this;
timer.Elapsed += new System.Timers.ElapsedEventHandler(timer_Elapsed);
timer.Start();
}
public Car(
b2World b2world,
double width,
double length,
double[] position,
double angle,
double power,
double max_steer_angle,
double max_speed,
Wheel[] wheels
)
{
this.wheels = wheels;
// /*
// pars is an object with possible attributes:
// width - width of the car in meters
// length - length of the car in meters
// position - starting position of the car, array [x, y] in meters
// angle - starting angle of the car, degrees
// max_steer_angle - maximum angle the wheels turn when steering, degrees
// max_speed - maximum speed of the car, km/h
// power - engine force, in newtons, that is applied to EACH powered wheel
// wheels - wheel definitions: [{x, y, rotatable, powered}}, ...] where
// x is wheel position in meters relative to car body center
// y is wheel position in meters relative to car body center
// revolving - boolean, does this turn rotate when steering?
// powered - is force applied to this wheel when accelerating/braking?
// */
// this.max_steer_angle=pars.max_steer_angle;
// this.max_speed=pars.max_speed;
// this.power=pars.power;
var wheel_angle = 0.0;//keep track of current wheel angle relative to car.
// //when steering left/right, angle will be decreased/increased gradually over 200ms to prevent jerkyness.
//initialize body
var def = new b2BodyDef();
def.type = b2Body.b2_dynamicBody;
def.position = new b2Vec2(position[0], position[1]);
def.angle = angle.DegreesToRadians();
def.linearDamping = 0.55; //gradually reduces velocity, makes the car reduce speed slowly if neither accelerator nor brake is pressed
def.bullet = true; //dedicates more time to collision detection - car travelling at high speeds at low framerates otherwise might teleport through obstacles.
def.angularDamping = 0.3;
this.body = b2world.CreateBody(def);
//initialize shape
var fixdef = new b2FixtureDef();
fixdef.density = 1.0;
fixdef.friction = 0.3; //friction when rubbing agaisnt other shapes
fixdef.restitution = 0.4; //amount of force feedback when hitting something. >0 makes the car bounce off, it's fun!
var fixdef_shape = new b2PolygonShape();
fixdef.shape = fixdef_shape;
fixdef_shape.SetAsBox(width / 2, length / 2);
this.fix = body.CreateFixture(fixdef);
//initialize wheels
foreach (var item in wheels)
{
item.Initialize(this);
}
//return array of wheels that turn when steering
IEnumerable <Wheel> getRevolvingWheels = from w in wheels where w.revolving select w;
// //return array of powered wheels
IEnumerable <Wheel> getPoweredWheels = from w in wheels where w.powered select w;
#region setSpeed
Action <double> setSpeed = (speed) =>
{
/*
* speed - speed in kilometers per hour
*/
var velocity0 = this.body.GetLinearVelocity();
//Console.WriteLine("car setSpeed velocity0 " + new { velocity0.x, velocity0.y });
var velocity2 = vectors.unit(new[] { velocity0.x, velocity0.y });
//Console.WriteLine("car setSpeed velocity2 " + new { x = velocity2[0], y = velocity2[1] });
var velocity = new b2Vec2(
velocity2[0] * ((speed * 1000.0) / 3600.0),
velocity2[1] * ((speed * 1000.0) / 3600.0)
);
//Console.WriteLine("car setSpeed SetLinearVelocity " + new { velocity.x, velocity.y });
this.body.SetLinearVelocity(velocity);
};
#endregion
#region getSpeedKMH
this.getSpeedKMH = delegate
{
var velocity = this.body.GetLinearVelocity();
var len = vectors.len(new double[] { velocity.x, velocity.y });
return((len / 1000.0) * 3600.0);
};
#endregion
#region getLocalVelocity
Func <double[]> getLocalVelocity = delegate
{
/*
* returns car's velocity vector relative to the car
*/
var retv = this.body.GetLocalVector(this.body.GetLinearVelocityFromLocalPoint(new b2Vec2(0, 0)));
return(new double[] { retv.x, retv.y });
};
#endregion
#region update
this.update = (msDuration) =>
{
#region 1. KILL SIDEWAYS VELOCITY
//kill sideways velocity for all wheels
for (var i = 0; i < wheels.Length; i++)
{
wheels[i].killSidewaysVelocity();
}
#endregion
#region 2. SET WHEEL ANGLE
//calculate the change in wheel's angle for this update, assuming the wheel will reach is maximum angle from zero in 200 ms
var incr = (max_steer_angle / 200.0) * msDuration;
if (steer_right == STEER_RIGHT)
{
wheel_angle = Math.Min(Math.Max(wheel_angle, 0) + incr, max_steer_angle); //increment angle without going over max steer
}
else if (steer_left == STEER_LEFT)
{
wheel_angle = Math.Max(Math.Min(wheel_angle, 0) - incr, -max_steer_angle); //decrement angle without going over max steer
}
else
{
wheel_angle = 0;
}
//update revolving wheels
getRevolvingWheels.WithEach(
w => w.setAngle(wheel_angle)
);
#endregion
#region 3. APPLY FORCE TO WHEELS
var base_vect = new double[2]; //vector pointing in the direction force will be applied to a wheel ; relative to the wheel.
//if accelerator is pressed down and speed limit has not been reached, go forwards
var lessthanlimit = (getSpeedKMH() < max_speed);
var flag1 = (accelerate == ACC_ACCELERATE) && lessthanlimit;
if (flag1)
{
base_vect = new double[] { 0, -1 };
}
else if (accelerate == ACC_BRAKE)
{
//braking, but still moving forwards - increased force
if (getLocalVelocity()[1] < 0)
{
base_vect = new double[] { 0, 1.3 };
}
//going in reverse - less force
else
{
base_vect = new double[] { 0, 0.7 };
}
}
else
{
base_vect[0] = 0;
base_vect[1] = 0;
}
//multiply by engine power, which gives us a force vector relative to the wheel
var fvect = new double[] {
power *base_vect[0],
power *base_vect[1]
};
//apply force to each wheel
getPoweredWheels.WithEachIndex(
(w, i) =>
{
var wp = w.body.GetWorldCenter();
var wf = w.body.GetWorldVector(new b2Vec2(fvect[0], fvect[1]));
//Console.WriteLine("getPoweredWheels ApplyForce #" + i);
w.body.ApplyForce(wf, wp);
}
);
//if going very slow, stop - to prevent endless sliding
var veryslow = (getSpeedKMH() < 4);
var flag2 = veryslow && (accelerate == ACC_NONE);
if (flag2)
{
//Console.WriteLine("setSpeed 0");
setSpeed(0);
}
#endregion
};
#endregion
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
public static float b2FindMaxSeparation(out int edgeIndex,
b2PolygonShape poly1, ref b2Transform xf1,
b2PolygonShape poly2, ref b2Transform xf2)
{
int count1 = poly1.VertexCount;
b2Vec2[] normals1 = poly1.Normals;
// Vector pointing from the centroid of poly1 to the centroid of poly2.
b2Vec2 d = b2Math.b2Mul(xf2, poly2.Centroid) - b2Math.b2Mul(xf1, poly1.Centroid);
b2Vec2 dLocal1 = b2Math.b2MulT(xf1.q, d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
float maxDot = -b2Settings.b2_maxFloat;
for (int i = 0; i < count1; ++i)
{
float dot = b2Math.b2Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float sNext = b2EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
// Find the best edge and the search direction.
int bestEdge;
float bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
edgeIndex = edge;
return(s);
}
// Perform a local search for the best edge normal.
for (; ;)
{
if (increment == -1)
{
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
}
else
{
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
}
s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
edgeIndex = bestEdge;
return(bestSeparation);
}
/// Implement b2Shape.
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
public override bool RayCast(b2RayCastOutput output, b2RayCastInput input, b2Transform xf, int childIndex)
{
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = Utils.b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = Utils.b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = new b2Vec2(m_vertex1);
b2Vec2 v2 = new b2Vec2(m_vertex2);
b2Vec2 e = v2 - v1;
b2Vec2 normal = new b2Vec2(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float numerator = Utils.b2Dot(normal, v1 - p1);
float denominator = Utils.b2Dot(normal, d);
if (denominator == 0.0f)
{
return(false);
}
float t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return(false);
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float rr = Utils.b2Dot(r, r);
if (rr == 0.0f)
{
return(false);
}
float s = Utils.b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return(false);
}
output.fraction = t;
if (numerator > 0.0f)
{
output.normal = -Utils.b2Mul(xf.q, normal);
}
else
{
output.normal = Utils.b2Mul(xf.q, normal);
}
return(true);
}
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;
float mA = m_invMassA, mB = m_invMassB;
float iA = m_invIA, iB = m_invIB;
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);
// Compute the effective masses.
b2Vec2 rA = b2Math.b2Mul(qA, m_localAnchorA - m_localCenterA);
b2Vec2 rB = b2Math.b2Mul(qB, m_localAnchorB - m_localCenterB);
b2Vec2 d = cB + rB - cA - rA;
// Point to line raint
{
m_ay = b2Math.b2Mul(qA, m_localYAxisA);
m_sAy = b2Math.b2Cross(d + rA, m_ay);
m_sBy = b2Math.b2Cross(rB, m_ay);
m_mass = mA + mB + iA * m_sAy * m_sAy + iB * m_sBy * m_sBy;
if (m_mass > 0.0f)
{
m_mass = 1.0f / m_mass;
}
}
// Spring raint
m_springMass = 0.0f;
m_bias = 0.0f;
m_gamma = 0.0f;
if (m_frequencyHz > 0.0f)
{
m_ax = b2Math.b2Mul(qA, m_localXAxisA);
m_sAx = b2Math.b2Cross(d + rA, m_ax);
m_sBx = b2Math.b2Cross(rB, m_ax);
float invMass = mA + mB + iA * m_sAx * m_sAx + iB * m_sBx * m_sBx;
if (invMass > 0.0f)
{
m_springMass = 1.0f / invMass;
float C = b2Math.b2Dot(d, m_ax);
// Frequency
float omega = 2.0f * (float)Math.PI * m_frequencyHz;
// Damping coefficient
float dx = 2.0f * m_springMass * m_dampingRatio * omega;
// Spring stiffness
float k = m_springMass * omega * omega;
// magic formulas
float h = data.step.dt;
m_gamma = h * (dx + h * k);
if (m_gamma > 0.0f)
{
m_gamma = 1.0f / m_gamma;
}
m_bias = C * h * k * m_gamma;
m_springMass = invMass + m_gamma;
if (m_springMass > 0.0f)
{
m_springMass = 1.0f / m_springMass;
}
}
}
else
{
m_springImpulse = 0.0f;
}
// Rotational motor
if (m_enableMotor)
{
m_motorMass = iA + iB;
if (m_motorMass > 0.0f)
{
m_motorMass = 1.0f / m_motorMass;
}
}
else
{
m_motorMass = 0.0f;
m_motorImpulse = 0.0f;
}
if (data.step.warmStarting)
{
// Account for variable time step.
m_impulse *= data.step.dtRatio;
m_springImpulse *= data.step.dtRatio;
m_motorImpulse *= data.step.dtRatio;
b2Vec2 P = m_impulse * m_ay + m_springImpulse * m_ax;
float LA = m_impulse * m_sAy + m_springImpulse * m_sAx + m_motorImpulse;
float LB = m_impulse * m_sBy + m_springImpulse * m_sBx + m_motorImpulse;
vA -= m_invMassA * P;
wA -= m_invIA * LA;
vB += m_invMassB * P;
wB += m_invIB * LB;
}
else
{
m_impulse = 0.0f;
m_springImpulse = 0.0f;
m_motorImpulse = 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 virtual bool TestPoint(b2Vec2 p)
{
return(m_shape.TestPoint(m_body.Transform, p));
}
public override bool TestPoint(ref b2Transform xf, b2Vec2 p)
{
return(false);
}
public override void SolveVelocityConstraints(b2TimeStep step)
{
b2Body bA = m_bodyA;
b2Body bB = m_bodyB;
b2Mat22 tMat;
float tX;
float newImpulse;
float r1X;
float r1Y;
float r2X;
float r2Y;
b2Vec2 v1 = bA.m_linearVelocity;
float w1 = bA.m_angularVelocity;
b2Vec2 v2 = bB.m_linearVelocity;
float w2 = bB.m_angularVelocity;
float m1 = bA.m_invMass;
float m2 = bB.m_invMass;
float i1 = bA.m_invI;
float i2 = bB.m_invI;
// Solve motor constraint.
if (m_enableMotor && m_limitState != e_equalLimits)
{
float Cdot = w2 - w1 - m_motorSpeed;
float impulse = m_motorMass * (-Cdot);
float oldImpulse = m_motorImpulse;
float maxImpulse = step.dt * m_maxMotorTorque;
m_motorImpulse = b2Math.Clamp(m_motorImpulse + impulse, -maxImpulse, maxImpulse);
impulse = m_motorImpulse - oldImpulse;
w1 -= i1 * impulse;
w2 += i2 * impulse;
}
// Solve limit constraint.
if (m_enableLimit && m_limitState != e_inactiveLimit)
{
//b2Vec2 r1 = b2Mul(bA->m_xf.R, m_localAnchor1 - bA->GetLocalCenter());
tMat = bA.m_xf.R;
r1X = m_localAnchor1.x - bA.m_sweep.localCenter.x;
r1Y = m_localAnchor1.y - bA.m_sweep.localCenter.y;
tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
r1X = tX;
//b2Vec2 r2 = b2Mul(bB->m_xf.R, m_localAnchor2 - bB->GetLocalCenter());
tMat = bB.m_xf.R;
r2X = m_localAnchor2.x - bB.m_sweep.localCenter.x;
r2Y = m_localAnchor2.y - bB.m_sweep.localCenter.y;
tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
r2X = tX;
// Solve point-to-point constraint
//b2Vec2 Cdot1 = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
float Cdot1X = v2.x + (-w2 * r2Y) - v1.x - (-w1 * r1Y);
float Cdot1Y = v2.y + (w2 * r2X) - v1.y - (w1 * r1X);
float Cdot2 = w2 - w1;
m_mass.Solve33(impulse3, -Cdot1X, -Cdot1Y, -Cdot2);
if (m_limitState == e_equalLimits)
{
m_impulse.Add(impulse3);
}
else if (m_limitState == e_atLowerLimit)
{
newImpulse = m_impulse.z + impulse3.z;
if (newImpulse < 0.0f)
{
m_mass.Solve22(reduced, -Cdot1X, -Cdot1Y);
impulse3.x = reduced.x;
impulse3.y = reduced.y;
impulse3.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
}
else if (m_limitState == e_atUpperLimit)
{
newImpulse = m_impulse.z + impulse3.z;
if (newImpulse > 0.0f)
{
m_mass.Solve22(reduced, -Cdot1X, -Cdot1Y);
impulse3.x = reduced.x;
impulse3.y = reduced.y;
impulse3.z = -m_impulse.z;
m_impulse.x += reduced.x;
m_impulse.y += reduced.y;
m_impulse.z = 0.0f;
}
}
v1.x -= m1 * impulse3.x;
v1.y -= m1 * impulse3.y;
w1 -= i1 * (r1X * impulse3.y - r1Y * impulse3.x + impulse3.z);
v2.x += m2 * impulse3.x;
v2.y += m2 * impulse3.y;
w2 += i2 * (r2X * impulse3.y - r2Y * impulse3.x + impulse3.z);
}
else
{
//b2Vec2 r1 = b2Mul(bA->m_xf.R, m_localAnchor1 - bA->GetLocalCenter());
tMat = bA.m_xf.R;
r1X = m_localAnchor1.x - bA.m_sweep.localCenter.x;
r1Y = m_localAnchor1.y - bA.m_sweep.localCenter.y;
tX = (tMat.col1.x * r1X + tMat.col2.x * r1Y);
r1Y = (tMat.col1.y * r1X + tMat.col2.y * r1Y);
r1X = tX;
//b2Vec2 r2 = b2Mul(bB->m_xf.R, m_localAnchor2 - bB->GetLocalCenter());
tMat = bB.m_xf.R;
r2X = m_localAnchor2.x - bB.m_sweep.localCenter.x;
r2Y = m_localAnchor2.y - bB.m_sweep.localCenter.y;
tX = (tMat.col1.x * r2X + tMat.col2.x * r2Y);
r2Y = (tMat.col1.y * r2X + tMat.col2.y * r2Y);
r2X = tX;
//b2Vec2 Cdot = v2 + b2Cross(w2, r2) - v1 - b2Cross(w1, r1);
float CdotX = v2.x + (-w2 * r2Y) - v1.x - (-w1 * r1Y);
float CdotY = v2.y + (w2 * r2X) - v1.y - (w1 * r1X);
m_mass.Solve22(impulse2, -CdotX, -CdotY);
m_impulse.x += impulse2.x;
m_impulse.y += impulse2.y;
v1.x -= m1 * impulse2.x;
v1.y -= m1 * impulse2.y;
//w1 -= i1 * b2Cross(r1, impulse2);
w1 -= i1 * (r1X * impulse2.y - r1Y * impulse2.x);
v2.x += m2 * impulse2.x;
v2.y += m2 * impulse2.y;
//w2 += i2 * b2Cross(r2, impulse2);
w2 += i2 * (r2X * impulse2.y - r2Y * impulse2.x);
}
bA.m_linearVelocity.SetV(v1);
bA.m_angularVelocity = w1;
bB.m_linearVelocity.SetV(v2);
bB.m_angularVelocity = w2;
}
public b2Vec2 Skew()
{
b2Vec2 ret = new b2Vec2(Box2dPINVOKE.b2Vec2_Skew(swigCPtr), true);
return(ret);
}
/// Shift the world origin. Useful for large worlds.
/// The shift formula is: position -= newOrigin
/// @param newOrigin the new origin with respect to the old origin
public void ShiftOrigin(b2Vec2 newOrigin)
{
m_tree.ShiftOrigin(newOrigin);
}
public b2Vec2 Minus()
{
b2Vec2 ret = new b2Vec2(Box2dPINVOKE.b2Vec2_Minus(swigCPtr), true);
return(ret);
}
public b2Vec2 GetGravity()
{
b2Vec2 ret = new b2Vec2(Box2DPINVOKE.b2World_GetGravity(swigCPtr), true);
return(ret);
}
void MakeCircle(b2Vec2 pos, float radius)
{
// Define the dynamic body. We set its position and call the body factory.
b2BodyDef bodyDef = new b2BodyDef();
bodyDef.type = b2BodyType.b2_dynamicBody;
bodyDef.position = pos;
var body = world.CreateBody(bodyDef);
// Define another box shape for our dynamic body.
b2CircleShape dynamicBox = new b2CircleShape();
dynamicBox.m_radius = radius;
// Define the dynamic body fixture.
b2FixtureDef fixtureDef = new b2FixtureDef();
fixtureDef.shape = dynamicBox;
// Set the box density to be non-zero, so it will be dynamic.
fixtureDef.density = 1.0f;
// Override the default friction.
fixtureDef.friction = 0.3f;
// Add the shape to the body.
body.CreateFixture(fixtureDef);
}
/// Compute the collision manifold between a polygon and a circle.
public static void b2CollidePolygonAndCircle(ref b2Manifold manifold,
b2PolygonShape polygonA, ref b2Transform xfA,
b2CircleShape circleB, ref b2Transform xfB)
{
manifold.pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Math.b2Mul(xfB, circleB.Position);
b2Vec2 cLocal = b2Math.b2MulT(xfA, c);
// Find the min separating edge.
int normalIndex = 0;
float separation = -b2Settings.b2_maxFloat;
float radius = polygonA.Radius + circleB.Radius;
int vertexCount = polygonA.VertexCount;
b2Vec2[] vertices = polygonA.Vertices;
b2Vec2[] normals = polygonA.Normals;
for (int i = 0; i < vertexCount; ++i)
{
float s = b2Math.b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int vertIndex1 = normalIndex;
int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2Settings.b2_epsilon)
{
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[normalIndex];
manifold.localPoint = 0.5f * (v1 + v2);
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
return;
}
// Compute barycentric coordinates
float u1 = b2Math.b2Dot(cLocal - v1, v2 - v1);
float u2 = b2Math.b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v1;
manifold.localNormal.Normalize();
manifold.localPoint = v1;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else if (u2 <= 0.0f)
{
if (b2Math.b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = cLocal - v2;
manifold.localNormal.Normalize();
manifold.localPoint = v2;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
separation = b2Math.b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation > radius)
{
return;
}
manifold.pointCount = 1;
manifold.type = b2ManifoldType.e_faceA;
manifold.localNormal = normals[vertIndex1];
manifold.localPoint = faceCenter;
manifold.points[0].localPoint = circleB.Position;
manifold.points[0].id = b2ContactFeature.Zero;
}
}
public void DrawPoint(b2Vec2 p, float size, b2Color 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);
}
/// Compute the collision manifold between two polygons.
// Find edge normal of max separation on A - return if separating axis is found
// Find edge normal of max separation on B - return if separation axis is found
// Choose reference edge as min(minA, minB)
// Find incident edge
// Clip
// The normal points from 1 to 2
public static void b2CollidePolygons(ref b2Manifold manifold,
b2PolygonShape polyA, ref b2Transform xfA,
b2PolygonShape polyB, ref b2Transform xfB)
{
manifold.pointCount = 0;
float totalRadius = polyA.Radius + polyB.Radius;
int edgeA = 0;
float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB);
if (separationA > totalRadius)
{
return;
}
int edgeB = 0;
float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA);
if (separationB > totalRadius)
{
return;
}
b2PolygonShape poly1; // reference polygon
b2PolygonShape poly2; // incident polygon
b2Transform xf1, xf2;
int edge1; // reference edge
byte flip;
const float k_relativeTol = 0.98f;
const float k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold.type = b2ManifoldType.e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold.type = b2ManifoldType.e_faceA;
flip = 0;
}
b2ClipVertex[] incidentEdge = new b2ClipVertex[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int count1 = poly1.VertexCount;
b2Vec2[] vertices1 = poly1.Vertices;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = b2Math.b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent);
b2Vec2 normal = b2Math.b2Cross(tangent, 1.0f);
v11 = b2Math.b2Mul(xf1, v11);
v12 = b2Math.b2Mul(xf1, v12);
// Face offset.
float frontOffset = b2Math.b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float sideOffset1 = -b2Math.b2Dot(tangent, v11) + totalRadius;
float sideOffset2 = b2Math.b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex[] clipPoints1 = new b2ClipVertex[2];
b2ClipVertex[] clipPoints2 = new b2ClipVertex[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1);
if (np < 2)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)iv2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold.localNormal = localNormal;
manifold.localPoint = planePoint;
int pointCount = 0;
for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i)
{
float separation = b2Math.b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint cp = manifold.points[pointCount];
cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v);
cp.id = clipPoints2[i].id;
if (flip != 0)
{
// Swap features
b2ContactFeature cf = cp.id;
cp.id.indexA = cf.indexB;
cp.id.indexB = cf.indexA;
cp.id.typeA = cf.typeB;
cp.id.typeB = cf.typeA;
}
manifold.points[pointCount] = cp;
++pointCount;
}
}
manifold.pointCount = pointCount;
}
public override void DrawSegment(b2Vec2 p1, b2Vec2 p2, b2Color color)
{
Gl.glColor3f(color.r, color.g, color.b);
Gl.glBegin(Gl.GL_LINES);
Gl.glVertex2f(p1.x, p1.y);
Gl.glVertex2f(p2.x, p2.y);
Gl.glEnd();
}
/// 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 override void DrawSolidPolygon(b2Vec2[] vertices, int vertexCount, b2Color color)
{
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 < vertexCount; ++i)
{
Gl.glVertex2f(vertices[i].x, vertices[i].y);
}
Gl.glEnd();
Gl.glDisable(Gl.GL_BLEND);
Gl.glColor4f(color.r, color.g, color.b, 1.0f);
Gl.glBegin(Gl.GL_LINE_LOOP);
for (int i = 0; i < vertexCount; ++i)
{
Gl.glVertex2f(vertices[i].x, vertices[i].y);
}
Gl.glEnd();
}
public b2RayCastOutput(b2Vec2 b, float f)
{
normal = b;
fraction = f;
}
public Car(
b2World b2world,
double width,
double length,
double[] position,
double angle,
double power,
double max_steer_angle,
double max_speed,
Wheel[] wheels
)
{
// /*
// pars is an object with possible attributes:
// width - width of the car in meters
// length - length of the car in meters
// position - starting position of the car, array [x, y] in meters
// angle - starting angle of the car, degrees
// max_steer_angle - maximum angle the wheels turn when steering, degrees
// max_speed - maximum speed of the car, km/h
// power - engine force, in newtons, that is applied to EACH powered wheel
// wheels - wheel definitions: [{x, y, rotatable, powered}}, ...] where
// x is wheel position in meters relative to car body center
// y is wheel position in meters relative to car body center
// revolving - boolean, does this turn rotate when steering?
// powered - is force applied to this wheel when accelerating/braking?
// */
// this.max_steer_angle=pars.max_steer_angle;
// this.max_speed=pars.max_speed;
// this.power=pars.power;
var wheel_angle = 0.0;//keep track of current wheel angle relative to car.
// //when steering left/right, angle will be decreased/increased gradually over 200ms to prevent jerkyness.
//initialize body
var def = new b2BodyDef();
def.type = b2Body.b2_dynamicBody;
def.position = new b2Vec2(position[0], position[1]);
def.angle = angle.DegreesToRadians();
def.linearDamping = 0.15; //gradually reduces velocity, makes the car reduce speed slowly if neither accelerator nor brake is pressed
def.bullet = true; //dedicates more time to collision detection - car travelling at high speeds at low framerates otherwise might teleport through obstacles.
def.angularDamping = 0.3;
this.body = b2world.CreateBody(def);
//initialize shape
var fixdef = new b2FixtureDef();
fixdef.density = 1.0;
fixdef.friction = 0.3; //friction when rubbing agaisnt other shapes
fixdef.restitution = 0.4; //amount of force feedback when hitting something. >0 makes the car bounce off, it's fun!
var fixdef_shape = new b2PolygonShape();
fixdef.shape = fixdef_shape;
fixdef_shape.SetAsBox(width / 2, length / 2);
body.CreateFixture(fixdef);
//initialize wheels
foreach (var item in wheels)
{
item.Initialize(this);
}
//return array of wheels that turn when steering
IEnumerable<Wheel> getRevolvingWheels = from w in wheels where w.revolving select w;
// //return array of powered wheels
IEnumerable<Wheel> getPoweredWheels = from w in wheels where w.powered select w;
#region setSpeed
Action<double> setSpeed = (speed) =>
{
/*
speed - speed in kilometers per hour
*/
var velocity0 = this.body.GetLinearVelocity();
//Console.WriteLine("car setSpeed velocity0 " + new { velocity0.x, velocity0.y });
var velocity2 = vectors.unit(new[] { velocity0.x, velocity0.y });
//Console.WriteLine("car setSpeed velocity2 " + new { x = velocity2[0], y = velocity2[1] });
var velocity = new b2Vec2(
velocity2[0] * ((speed * 1000.0) / 3600.0),
velocity2[1] * ((speed * 1000.0) / 3600.0)
);
//Console.WriteLine("car setSpeed SetLinearVelocity " + new { velocity.x, velocity.y });
this.body.SetLinearVelocity(velocity);
};
#endregion
#region getSpeedKMH
Func<double> getSpeedKMH = delegate
{
var velocity = this.body.GetLinearVelocity();
var len = vectors.len(new double[] { velocity.x, velocity.y });
return (len / 1000.0) * 3600.0;
};
#endregion
#region getLocalVelocity
Func<double[]> getLocalVelocity = delegate
{
/*
returns car's velocity vector relative to the car
*/
var retv = this.body.GetLocalVector(this.body.GetLinearVelocityFromLocalPoint(new b2Vec2(0, 0)));
return new double[] { retv.x, retv.y };
};
#endregion
#region update
this.update = (msDuration) =>
{
#region 1. KILL SIDEWAYS VELOCITY
//kill sideways velocity for all wheels
for (var i = 0; i < wheels.Length; i++)
{
wheels[i].killSidewaysVelocity();
}
#endregion
#region 2. SET WHEEL ANGLE
//calculate the change in wheel's angle for this update, assuming the wheel will reach is maximum angle from zero in 200 ms
var incr = (max_steer_angle / 200.0) * msDuration;
if (steer == STEER_RIGHT)
{
wheel_angle = Math.Min(Math.Max(wheel_angle, 0) + incr, max_steer_angle); //increment angle without going over max steer
}
else if (steer == STEER_LEFT)
{
wheel_angle = Math.Max(Math.Min(wheel_angle, 0) - incr, -max_steer_angle); //decrement angle without going over max steer
}
else
{
wheel_angle = 0;
}
//update revolving wheels
getRevolvingWheels.WithEach(
w => w.setAngle(wheel_angle)
);
#endregion
#region 3. APPLY FORCE TO WHEELS
var base_vect = new double[2]; //vector pointing in the direction force will be applied to a wheel ; relative to the wheel.
//if accelerator is pressed down and speed limit has not been reached, go forwards
var lessthanlimit = (getSpeedKMH() < max_speed);
var flag1 = (accelerate == ACC_ACCELERATE) && lessthanlimit;
if (flag1)
{
base_vect = new double[] { 0, -1 };
}
else if (accelerate == ACC_BRAKE)
{
//braking, but still moving forwards - increased force
if (getLocalVelocity()[1] < 0)
{
base_vect = new double[] { 0, 1.3 };
}
//going in reverse - less force
else
{
base_vect = new double[] { 0, 0.7 };
}
}
else
{
base_vect[0] = 0;
base_vect[1] = 0;
}
//multiply by engine power, which gives us a force vector relative to the wheel
var fvect = new double[] {
power * base_vect[0],
power * base_vect[1]
};
//apply force to each wheel
getPoweredWheels.WithEachIndex(
(w, i) =>
{
var wp = w.body.GetWorldCenter();
var wf = w.body.GetWorldVector(new b2Vec2(fvect[0], fvect[1]));
//Console.WriteLine("getPoweredWheels ApplyForce #" + i);
w.body.ApplyForce(wf, wp);
}
);
//if going very slow, stop - to prevent endless sliding
var veryslow = (getSpeedKMH() < 4);
var flag2 = veryslow && (accelerate == ACC_NONE);
if (flag2)
{
//Console.WriteLine("setSpeed 0");
setSpeed(0);
}
#endregion
};
#endregion
}
private void initPhysics()
{
CCSize s = Layer.VisibleBoundsWorldspace.Size;
var gravity = new b2Vec2(0.0f, -10.0f);
_world = new b2World(gravity);
float debugWidth = s.Width / PTM_RATIO * 2f;
float debugHeight = s.Height / PTM_RATIO * 2f;
//CCBox2dDraw debugDraw = new CCBox2dDraw(new b2Vec2(debugWidth / 2f + 10, s.Height - debugHeight - 10), 2);
//debugDraw.AppendFlags(b2DrawFlags.e_shapeBit);
//_world.SetDebugDraw(debugDraw);
_world.SetAllowSleeping(true);
_world.SetContinuousPhysics(true);
//m_debugDraw = new GLESDebugDraw( PTM_RATIO );
//world->SetDebugDraw(m_debugDraw);
//uint32 flags = 0;
//flags += b2Draw::e_shapeBit;
// flags += b2Draw::e_jointBit;
// flags += b2Draw::e_aabbBit;
// flags += b2Draw::e_pairBit;
// flags += b2Draw::e_centerOfMassBit;
//m_debugDraw->SetFlags(flags);
// Call the body factory which allocates memory for the ground body
// from a pool and creates the ground box shape (also from a pool).
// The body is also added to the world.
b2BodyDef def = new b2BodyDef();
def.allowSleep = true;
def.position = b2Vec2.Zero;
def.type = b2BodyType.b2_staticBody;
b2Body groundBody = _world.CreateBody(def);
groundBody.SetActive(true);
// Define the ground box shape.
// bottom
b2EdgeShape groundBox = new b2EdgeShape();
groundBox.Set(b2Vec2.Zero, new b2Vec2(s.Width / PTM_RATIO, 0));
b2FixtureDef fd = new b2FixtureDef();
fd.shape = groundBox;
groundBody.CreateFixture(fd);
// top
groundBox = new b2EdgeShape();
groundBox.Set(new b2Vec2(0, s.Height / PTM_RATIO), new b2Vec2(s.Width / PTM_RATIO, s.Height / PTM_RATIO));
fd.shape = groundBox;
groundBody.CreateFixture(fd);
// left
groundBox = new b2EdgeShape();
groundBox.Set(new b2Vec2(0, s.Height / PTM_RATIO), b2Vec2.Zero);
fd.shape = groundBox;
groundBody.CreateFixture(fd);
// right
groundBox = new b2EdgeShape();
groundBox.Set(new b2Vec2(s.Width / PTM_RATIO, s.Height / PTM_RATIO), new b2Vec2(s.Width / PTM_RATIO, 0));
fd.shape = groundBox;
groundBody.CreateFixture(fd);
// _world.Dump();
}
