public override float computeSubmergedArea( ref Vector2 normal, float offset, ref Transform xf, out Vector2 sc )
{
sc = Vector2.Zero;
//Transform plane into shape co-ordinates
var normalL = MathUtils.mulT( xf.q, normal );
float offsetL = offset - Vector2.Dot( normal, xf.p );
float[] depths = new float[Settings.maxPolygonVertices];
int diveCount = 0;
int intoIndex = -1;
int outoIndex = -1;
bool lastSubmerged = false;
int i;
for( i = 0; i < vertices.Count; i++ )
{
depths[i] = Vector2.Dot( normalL, vertices[i] ) - offsetL;
bool isSubmerged = depths[i] < -Settings.epsilon;
if( i > 0 )
{
if( isSubmerged )
{
if( !lastSubmerged )
{
intoIndex = i - 1;
diveCount++;
}
}
else
{
if( lastSubmerged )
{
outoIndex = i - 1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch( diveCount )
{
case 0:
if( lastSubmerged )
{
//Completely submerged
sc = MathUtils.mul( ref xf, massData.centroid );
return massData.mass / density;
}
//Completely dry
return 0;
case 1:
if( intoIndex == -1 )
{
intoIndex = vertices.Count - 1;
}
else
{
outoIndex = vertices.Count - 1;
}
break;
}
int intoIndex2 = ( intoIndex + 1 ) % vertices.Count;
int outoIndex2 = ( outoIndex + 1 ) % vertices.Count;
float intoLambda = ( 0 - depths[intoIndex] ) / ( depths[intoIndex2] - depths[intoIndex] );
float outoLambda = ( 0 - depths[outoIndex] ) / ( depths[outoIndex2] - depths[outoIndex] );
Vector2 intoVec = new Vector2( vertices[intoIndex].X * ( 1 - intoLambda ) + vertices[intoIndex2].X * intoLambda, vertices[intoIndex].Y * ( 1 - intoLambda ) + vertices[intoIndex2].Y * intoLambda );
Vector2 outoVec = new Vector2( vertices[outoIndex].X * ( 1 - outoLambda ) + vertices[outoIndex2].X * outoLambda, vertices[outoIndex].Y * ( 1 - outoLambda ) + vertices[outoIndex2].Y * outoLambda );
//Initialize accumulator
float area = 0;
Vector2 center = new Vector2( 0, 0 );
Vector2 p2 = vertices[intoIndex2];
const float k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while( i != outoIndex2 )
{
i = ( i + 1 ) % vertices.Count;
Vector2 p3;
if( i == outoIndex2 )
p3 = outoVec;
else
p3 = vertices[i];
//Add the triangle formed by intoVec,p2,p3
{
Vector2 e1 = p2 - intoVec;
Vector2 e2 = p3 - intoVec;
float D = MathUtils.cross( e1, e2 );
float triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * ( intoVec + p2 + p3 );
}
p2 = p3;
}
//Normalize and transform centroid
center *= 1.0f / area;
sc = MathUtils.mul( ref xf, center );
return area;
}