public static void ComputeCentroid(out Vec2 centroid, System.Collections.Generic.IList <Vec2> vertices)
{
if (!(vertices.Count >= 2))
{
throw new ArgumentOutOfRangeException("vertices");
}
centroid = Vec2.Empty;
float area = 0.0f;
if (vertices.Count == 2)
{
centroid = 0.5f * (vertices[0] + vertices[1]);
return;
}
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = Vec2.Empty;
const float inv3 = 1.0f / 3.0f;
for (int i = 0; i < vertices.Count; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = vertices[i];
Vec2 p3 = i + 1 < vertices.Count ? vertices[i + 1] : vertices[0];
Vec2 e1 = p2 - p1;
Vec2 e2 = p3 - p1;
float D = e1.Cross(e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
centroid += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
if (!(area > float.Epsilon))
{
throw new Exception("Area of polygon is too small");
}
centroid *= 1.0f / area;
}
public override void ComputeMass(out MassData massData, float density)
{
if (VertexCount < 2)
{
throw new ArgumentOutOfRangeException("Vertice count less than 2");
}
// A line segment has zero mass.
if (VertexCount == 2)
{
massData = new MassData(0.0f,
0.5f * (Vertices[0] + Vertices[1]),
0.0f);
return;
}
Vec2 center = Vec2.Empty;
float area = 0.0f;
float I = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
Vec2 pRef = Vec2.Empty;
#if NO
// This code would put the reference point inside the polygon.
for (int i = 0; i < VertexCount; ++i)
{
pRef += Vertices[i];
}
pRef *= 1.0f / count;
#endif
const float k_inv3 = 1.0f / 3.0f;
for (int i = 0; i < VertexCount; ++i)
{
// Triangle vertices.
Vec2 p1 = pRef;
Vec2 p2 = Vertices[i];
Vec2 p3 = i + 1 < VertexCount ? Vertices[i + 1] : Vertices[0];
Vec2 e1 = p2 - p1;
Vec2 e2 = p3 - p1;
float D = e1.Cross(e2);
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (p1 + p2 + p3);
float px = p1.X, py = p1.Y;
float ex1 = e1.X, ey1 = e1.Y;
float ex2 = e2.X, ey2 = e2.Y;
float intx2 = k_inv3 * (0.25f * (ex1 * ex1 + ex2 * ex1 + ex2 * ex2) + (px * ex1 + px * ex2)) + 0.5f * px * px;
float inty2 = k_inv3 * (0.25f * (ey1 * ey1 + ey2 * ey1 + ey2 * ey2) + (py * ey1 + py * ey2)) + 0.5f * py * py;
I += D * (intx2 + inty2);
}
if (!(area > float.Epsilon))
{
throw new Exception("Area is too small");
}
massData = new MassData(density * area,
center * (1.0f / area),
density * I);
}
void Set(Vec2[] verts)
{
Vec2[] normals = new Vec2[verts.Length];
if (!(2 <= verts.Length && verts.Length <= Box2DSettings.b2_maxPolygonVertices))
{
throw new ArgumentOutOfRangeException("verts", "Vertice count is " + ((2 <= verts.Length) ? "less than 2" : "greater than " + Box2DSettings.b2_maxPolygonVertices.ToString()));
}
if (_autoReverse)
// Ensure the polygon is convex and the interior
// is to the left of each edge.
{
bool _reversed = false;
for (int i = 0; i < verts.Length; ++i)
{
int i1 = i;
int i2 = i + 1 < verts.Length ? i + 1 : 0;
Vec2 edge = verts[i2] - verts[i1];
for (int j = 0; j < verts.Length; ++j)
{
// Don't check vertices on the current edge.
if (j == i1 || j == i2)
{
continue;
}
Vec2 r = verts[j] - verts[i1];
// Your polygon is non-convex (it has an indentation) or
// has colinear edges.
float s = edge.Cross(r);
if (!(s > 0.0f))
{
_reversed = true;
verts = ReverseOrder(verts);
break;
}
}
if (_reversed)
{
break;
}
}
}
// Compute normals. Ensure the edges have non-zero length.
for (int i = 0; i < verts.Length; ++i)
{
int i1 = i;
int i2 = i + 1 < verts.Length ? i + 1 : 0;
Vec2 edge = verts[i2] - verts[i1];
if (!(edge.LengthSquared() > float.Epsilon * float.Epsilon))
{
throw new Exception("Edge has a close-to-zero length (vertices too close?)");
}
normals[i] = edge.Cross(1.0f);
normals[i].Normalize();
}
// Compute the polygon centroid.
ComputeCentroid(out _internalPolyShape.m_centroid, verts);
for (int i = 0; i < verts.Length; ++i)
{
_internalPolyShape.m_vertices[i] = verts[i];
_internalPolyShape.m_normals[i] = normals[i];
}
VertexCount = verts.Length;
}
