static b2Vec2 ComputeCentroid(const b2Vec2* vs, int32 count)
{
b2Assert(count >= 2);
b2Vec2 c; c.Set(0.0f, 0.0f);
float32 area = 0.0f;
if (count == 2)
{
c = 0.5f * (vs[0] + vs[1]);
return c;
}
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 pRef(0.0f, 0.0f);
#if 0
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < count; ++i)
{
pRef += vs[i];
}
pRef *= 1.0f / count;
#endif
const float32 inv3 = 1.0f / 3.0f;
for (int32 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;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
b2Assert(area > b2_epsilon);
c *= 1.0f / area;
return c;
}
//Taken from b2PolygonShape.cpp
b2Vec2 CustomContactListener::ComputeCentroid(vector<b2Vec2> vs, float& area)
{
int count = (int)vs.size();
b2Assert(count >= 3);
b2Vec2 c;
c.Set(0.0f, 0.0f);
area = 0.0f;
// pRef is the reference point for forming triangles.
// Its location doesnt change the result (except for rounding error).
b2Vec2 pRef(0.0f, 0.0f);
const float32 inv3 = 1.0f / 3.0f;
for (int32 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;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
if (area > b2_epsilon)
c *= 1.0f / area;
else
area = 0;
return c;
}
bool LHBodyShape::LHValidateCentroid(b2Vec2* vs, int count)
{
if(count > b2_maxPolygonVertices)
return false;
if(count < 3)
return false;
b2Vec2 c; c.Set(0.0f, 0.0f);
float32 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(0.0f, 0.0f);
#if 0
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < count; ++i)
{
pRef += vs[i];
}
pRef *= 1.0f / count;
#endif
const float32 inv3 = 1.0f / 3.0f;
for (int32 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;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
if(area < b2_epsilon)
{
return false;
}
int32 n = b2Min(count, b2_maxPolygonVertices);
// Perform welding and copy vertices into local buffer.
b2Vec2 ps[b2_maxPolygonVertices];
int32 tempCount = 0;
for (int32 i = 0; i < n; ++i)
{
b2Vec2 v = vs[i];
bool unique = true;
for (int32 j = 0; j < tempCount; ++j)
{
if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop)
{
unique = false;
break;
}
}
if (unique)
{
ps[tempCount++] = v;
}
}
n = tempCount;
if (n < 3)
{
return false;
}
return true;
}
void b2PolygonShape::ComputeMass(b2MassData* massData, float32 density) const
{
// 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.
b2Assert(m_vertexCount >= 2);
// A line segment has zero mass.
if (m_vertexCount == 2)
{
massData->center = 0.5f * (m_vertices[0] + m_vertices[1]);
massData->mass = 0.0f;
massData->I = 0.0f;
return;
}
b2Vec2 center; center.Set(0.0f, 0.0f);
float32 area = 0.0f;
float32 I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 pRef(0.0f, 0.0f);
#if 0
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < m_vertexCount; ++i)
{
pRef += m_vertices[i];
}
pRef *= 1.0f / count;
#endif
const float32 k_inv3 = 1.0f / 3.0f;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// Triangle vertices.
b2Vec2 p1 = pRef;
b2Vec2 p2 = m_vertices[i];
b2Vec2 p3 = i + 1 < m_vertexCount ? m_vertices[i+1] : m_vertices[0];
b2Vec2 e1 = p2 - p1;
b2Vec2 e2 = p3 - p1;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (p1 + p2 + p3);
float32 px = p1.x, py = p1.y;
float32 ex1 = e1.x, ey1 = e1.y;
float32 ex2 = e2.x, ey2 = e2.y;
float32 intx2 = k_inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
float32 inty2 = k_inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
I += D * (intx2 + inty2);
}
// Total mass
massData->mass = density * area;
// Center of mass
b2Assert(area > b2_epsilon);
center *= 1.0f / area;
massData->center = center;
// Inertia tensor relative to the local origin.
massData->I = density * I;
}
bool Box2dHelper::areVerticesAcceptable(b2Vec2* vertices, int count)
{
if (count < 3)
return false;
if (count > b2_maxPolygonVertices){
return false;
}
int32 i;
for (i = 0; i < count; i++){
int32 i1 = i;
int32 i2 = i + 1 < count ? i + 1 : 0;
b2Vec2 edge = vertices[i2] - vertices[i1];
if (edge.LengthSquared() <= b2_epsilon * b2_epsilon){
return false;
}
}
float32 area = 0.0f;
b2Vec2 pRef(0.0f, 0.0f);
for (i = 0; i < count; i++){
b2Vec2 p1 = pRef;
b2Vec2 p2 = vertices[i];
b2Vec2 p3 = i + 1 < count ? vertices[i + 1] : vertices[0];
b2Vec2 e1 = p2 - p1;
b2Vec2 e2 = p3 - p1;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
}
if (area <= 0.0001f)
{
return false;
}
float determinant;
float referenceDeterminant;
b2Vec2 v1 = vertices[0] - vertices[count - 1];
b2Vec2 v2 = vertices[1] - vertices[0];
referenceDeterminant = calculate_determinant_2x2(v1.x, v1.y, v2.x, v2.y);
for (i = 1; i < count - 1; i++)
{
v1 = v2;
v2 = vertices[i + 1] - vertices[i];
determinant = calculate_determinant_2x2(v1.x, v1.y, v2.x, v2.y);
if (referenceDeterminant * determinant < 0.0f)
{
return false;
}
}
v1 = v2;
v2 = vertices[0] - vertices[count - 1];
determinant = calculate_determinant_2x2(v1.x, v1.y, v2.x, v2.y);
if (referenceDeterminant * determinant < 0.0f)
{
return false;
}
return true;
}
// ============================================================================
/// Checks shape validity. Returns 0 if valid.
///\author Erin Catto, Daid
///\note Taken from Boxcuter, licensed n Box2D license.
int CheckPolyShape(const b2PolygonDef* poly)
{
if (!(3 <= poly->vertexCount && poly->vertexCount <= b2_maxPolygonVertices))
return -1;
b2Vec2 m_normals[poly->vertexCount];
// Compute normals. Ensure the edges have non-zero length.
for (int32 i = 0; i < poly->vertexCount; ++i)
{
int32 i1 = i;
int32 i2 = i + 1 < poly->vertexCount ? i + 1 : 0;
b2Vec2 edge = poly->vertices[i2] - poly->vertices[i1];
if (!(edge.LengthSquared() > B2_FLT_EPSILON * B2_FLT_EPSILON))
return -1;
m_normals[i] = b2Cross(edge, 1.0f);
m_normals[i].Normalize();
}
// Ensure the polygon is convex.
for (int32 i = 0; i < poly->vertexCount; ++i)
{
for (int32 j = 0; j < poly->vertexCount; ++j)
{
// Don't check vertices on the current edge.
if (j == i || j == (i + 1) % poly->vertexCount)
{
continue;
}
// Your polygon is non-convex (it has an indentation).
// Or your polygon is too skinny.
float32 s = b2Dot(m_normals[i], poly->vertices[j] - poly->vertices[i]);
if (!(s < -b2_linearSlop))
return -1;
}
}
// Ensure the polygon is counter-clockwise.
for (int32 i = 1; i < poly->vertexCount; ++i)
{
float32 cross = b2Cross(m_normals[i-1], m_normals[i]);
// Keep asinf happy.
cross = b2Clamp(cross, -1.0f, 1.0f);
// You have consecutive edges that are almost parallel on your polygon.
float32 angle = asinf(cross);
if (!(angle > b2_angularSlop))
return -1;
}
// Compute the polygon centroid.
b2Vec2 m_centroid; m_centroid.Set(0.0f, 0.0f);
float32 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(0.0f, 0.0f);
const float32 inv3 = 1.0f / 3.0f;
for (int32 i = 0; i < poly->vertexCount; ++i)
{
// Triangle vertices.
b2Vec2 p1 = pRef;
b2Vec2 p2 = poly->vertices[i];
b2Vec2 p3 = i + 1 < poly->vertexCount ? poly->vertices[i+1] : poly->vertices[0];
b2Vec2 e1 = p2 - p1;
b2Vec2 e2 = p3 - p1;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
m_centroid += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
if (!(area > B2_FLT_EPSILON))
return -1;
m_centroid *= 1.0f / area;
// Compute the oriented bounding box.
//ComputeOBB(&m_obb, m_vertices, m_vertexCount);
// Create core polygon shape by shifting edges inward.
// Also compute the min/max radius for CCD.
for (int32 i = 0; i < poly->vertexCount; ++i)
{
int32 i1 = i - 1 >= 0 ? i - 1 : poly->vertexCount - 1;
int32 i2 = i;
b2Vec2 n1 = m_normals[i1];
b2Vec2 n2 = m_normals[i2];
b2Vec2 v = poly->vertices[i] - m_centroid;
b2Vec2 d;
d.x = b2Dot(n1, v) - b2_toiSlop;
d.y = b2Dot(n2, v) - b2_toiSlop;
// Shifting the edge inward by b2_toiSlop should
// not cause the plane to pass the centroid.
// Your shape has a radius/extent less than b2_toiSlop.
if (!(d.x >= 0.0f))
return -1;
if (!(d.y >= 0.0f))
return -1;
}
return 0;
}
// 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.
static void PolyMass(b2MassData* massData, const b2Vec2* vs, int32 count, float32 rho)
{
b2Assert(count >= 3);
b2Vec2 center; center.Set(0.0f, 0.0f);
float32 area = 0.0f;
float32 I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 pRef(0.0f, 0.0f);
#if 0
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < count; ++i)
{
pRef += vs[i];
}
pRef *= 1.0f / count;
#endif
const float32 inv3 = 1.0f / 3.0f;
for (int32 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;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * inv3 * (p1 + p2 + p3);
float32 px = p1.x, py = p1.y;
float32 ex1 = e1.x, ey1 = e1.y;
float32 ex2 = e2.x, ey2 = e2.y;
float32 intx2 = inv3 * (0.25f * (ex1*ex1 + ex2*ex1 + ex2*ex2) + (px*ex1 + px*ex2)) + 0.5f*px*px;
float32 inty2 = inv3 * (0.25f * (ey1*ey1 + ey2*ey1 + ey2*ey2) + (py*ey1 + py*ey2)) + 0.5f*py*py;
I += D * (intx2 + inty2);
}
// Total mass
massData->mass = rho * area;
// Center of mass
b2Assert(area > FLT_EPSILON);
center *= 1.0f / area;
massData->center = center;
// Inertia tensor relative to the center.
I = rho * (I - area * b2Dot(center, center));
massData->I = I;
}
