float32 b2PolygonShape::ComputeSubmergedArea( const b2Vec2& normal,
float32 offset,
const b2XForm& xf,
b2Vec2* c) const
{
//Transform plane into shape co-ordinates
b2Vec2 normalL = b2MulT(xf.R,normal);
float32 offsetL = offset - b2Dot(normal,xf.position);
float32 depths[b2_maxPolygonVertices];
int32 diveCount = 0;
int32 intoIndex = -1;
int32 outoIndex = -1;
bool lastSubmerged = false;
int32 i;
for(i=0;i<m_vertexCount;i++){
depths[i] = b2Dot(normalL,m_vertices[i]) - offsetL;
bool isSubmerged = depths[i]<-B2_FLT_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
b2MassData md;
ComputeMass(&md);
*c = b2Mul(xf,md.center);
return md.mass/m_density;
}else{
//Completely dry
return 0;
}
break;
case 1:
if(intoIndex==-1){
intoIndex = m_vertexCount-1;
}else{
outoIndex = m_vertexCount-1;
}
break;
}
int32 intoIndex2 = (intoIndex+1)%m_vertexCount;
int32 outoIndex2 = (outoIndex+1)%m_vertexCount;
float32 intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float32 outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
b2Vec2 intoVec( m_vertices[intoIndex].x*(1-intoLambda)+m_vertices[intoIndex2].x*intoLambda,
m_vertices[intoIndex].y*(1-intoLambda)+m_vertices[intoIndex2].y*intoLambda);
b2Vec2 outoVec( m_vertices[outoIndex].x*(1-outoLambda)+m_vertices[outoIndex2].x*outoLambda,
m_vertices[outoIndex].y*(1-outoLambda)+m_vertices[outoIndex2].y*outoLambda);
//Initialize accumulator
float32 area = 0;
b2Vec2 center(0,0);
b2Vec2 p2 = m_vertices[intoIndex2];
b2Vec2 p3;
float32 k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while(i!=outoIndex2){
i=(i+1)%m_vertexCount;
if(i==outoIndex2)
p3 = outoVec;
else
p3 = m_vertices[i];
//Add the triangle formed by intoVec,p2,p3
{
b2Vec2 e1 = p2 - intoVec;
b2Vec2 e2 = p3 - intoVec;
float32 D = b2Cross(e1, e2);
float32 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;
*c = b2Mul(xf,center);
return area;
}
float32 LH_b2BuoyancyController::ComputeSubmergedArea(b2Shape* shape,
const b2Vec2& normal,
float32 offset,
const b2Transform& xf,
b2Vec2* c,
float32 density) const
{
if(shape->GetType() == b2Shape::e_edge)
{
//Note that v0 is independant of any details of the specific edge
//We are relying on v0 being consistent between multiple edges of the same body
b2Vec2 v0 = offset * normal;
//b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;
b2Vec2 v1 = b2Mul(xf, ((b2EdgeShape*)shape)->m_vertex1);
b2Vec2 v2 = b2Mul(xf, ((b2EdgeShape*)shape)->m_vertex2);
float32 d1 = b2Dot(normal, v1) - offset;
float32 d2 = b2Dot(normal, v2) - offset;
if(d1>0)
{
if(d2>0)
{
return 0;
}
else
{
v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
}
}
else
{
if(d2>0)
{
v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
}
else
{
//Nothing
}
}
// v0,v1,v2 represents a fully submerged triangle
float32 k_inv3 = 1.0f / 3.0f;
// Area weighted centroid
*c = k_inv3 * (v0 + v1 + v2);
b2Vec2 e1 = v1 - v0;
b2Vec2 e2 = v2 - v0;
return 0.5f * b2Cross(e1, e2);
}
else if(shape->GetType() == b2Shape::e_polygon)
{
//Transform plane into shape co-ordinates
b2Vec2 normalL = b2MulT(xf.q,normal);
float32 offsetL = offset - b2Dot(normal,xf.p);
float32 depths[b2_maxPolygonVertices];
int32 diveCount = 0;
int32 intoIndex = -1;
int32 outoIndex = -1;
bool lastSubmerged = false;
int32 i;
for(i=0;i<((b2PolygonShape*)shape)->m_vertexCount;i++){
depths[i] = b2Dot(normalL,((b2PolygonShape*)shape)->m_vertices[i]) - offsetL;
bool isSubmerged = depths[i]<-FLT_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
b2MassData md;
((b2PolygonShape*)shape)->ComputeMass(&md, density);
*c = b2Mul(xf,md.center);
return md.mass/density;
}else{
//Completely dry
return 0;
}
break;
case 1:
if(intoIndex==-1){
intoIndex = ((b2PolygonShape*)shape)->m_vertexCount-1;
}else{
outoIndex = ((b2PolygonShape*)shape)->m_vertexCount-1;
}
break;
}
int32 intoIndex2 = (intoIndex+1)%((b2PolygonShape*)shape)->m_vertexCount;
int32 outoIndex2 = (outoIndex+1)%((b2PolygonShape*)shape)->m_vertexCount;
float32 intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float32 outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
b2Vec2 intoVec( ((b2PolygonShape*)shape)->m_vertices[intoIndex].x*(1-intoLambda)+((b2PolygonShape*)shape)->m_vertices[intoIndex2].x*intoLambda,
((b2PolygonShape*)shape)->m_vertices[intoIndex].y*(1-intoLambda)+((b2PolygonShape*)shape)->m_vertices[intoIndex2].y*intoLambda);
b2Vec2 outoVec( ((b2PolygonShape*)shape)->m_vertices[outoIndex].x*(1-outoLambda)+((b2PolygonShape*)shape)->m_vertices[outoIndex2].x*outoLambda,
((b2PolygonShape*)shape)->m_vertices[outoIndex].y*(1-outoLambda)+((b2PolygonShape*)shape)->m_vertices[outoIndex2].y*outoLambda);
//Initialize accumulator
float32 area = 0;
b2Vec2 center(0,0);
b2Vec2 p2 = ((b2PolygonShape*)shape)->m_vertices[intoIndex2];
b2Vec2 p3;
float32 k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while(i!=outoIndex2){
i=(i+1)%((b2PolygonShape*)shape)->m_vertexCount;
if(i==outoIndex2)
p3 = outoVec;
else
p3 = ((b2PolygonShape*)shape)->m_vertices[i];
//Add the triangle formed by intoVec,p2,p3
{
b2Vec2 e1 = p2 - intoVec;
b2Vec2 e2 = p3 - intoVec;
float32 D = b2Cross(e1, e2);
float32 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;
*c = b2Mul(xf,center);
return area;
}
else if(shape->GetType() == b2Shape::e_circle)
{
b2Vec2 p = b2Mul(xf,((b2CircleShape*)shape)->m_p);
float32 l = -(b2Dot(normal,p) - offset);
if(l<-((b2CircleShape*)shape)->m_radius+FLT_EPSILON){
//Completely dry
return 0;
}
if(l > ((b2CircleShape*)shape)->m_radius){
//Completely wet
*c = p;
return b2_pi*((b2CircleShape*)shape)->m_radius*((b2CircleShape*)shape)->m_radius;
}
//Magic
float32 r2 = ((b2CircleShape*)shape)->m_radius*((b2CircleShape*)shape)->m_radius;
float32 l2 = l*l;
//TODO: write b2Sqrt to handle fixed point case.
float32 area = r2 * (asin(l/((b2CircleShape*)shape)->m_radius) + b2_pi/2.0f)+ l * b2Sqrt(r2 - l2);
float32 com = -2.0f/3.0f*pow(r2-l2,1.5f)/area;
c->x = p.x + normal.x * com;
c->y = p.y + normal.y * com;
return area;
}
return 0;
}