public override void Step(b2TimeStep step)
{
if (m_bodyList == null)
{
return;
}
if (useWorldGravity)
{
gravity = GetWorld().GetGravity().Copy();
}
for (b2ControllerEdge i = m_bodyList; i != null; i = i.nextBody)
{
b2Body body = i.body;
if (body.IsAwake() == false)
{
//Buoyancy force is just a function of position,
//so unlike most forces, it is safe to ignore sleeping bodes
continue;
}
b2Vec2 areac = new b2Vec2();
b2Vec2 massc = new b2Vec2();
float area = 0.0f;
float mass = 0.0f;
for (b2Fixture fixture = body.GetFixtureList(); fixture != null; fixture = fixture.GetNext())
{
b2Vec2 sc = new b2Vec2();
float sarea = fixture.GetShape().ComputeSubmergedArea(normal, offset, body.GetTransform(), sc);
area += sarea;
areac.x += sarea * sc.x;
areac.y += sarea * sc.y;
float shapeDensity;
if (useDensity)
{
//TODO: Figure out what to do now density is gone
shapeDensity = 1.0f;
}
else
{
shapeDensity = 1.0f;
}
mass += sarea * shapeDensity;
massc.x += sarea * sc.x * shapeDensity;
massc.y += sarea * sc.y * shapeDensity;
}
areac.x /= area;
areac.y /= area;
massc.x /= mass;
massc.y /= mass;
if (area < float.MinValue)
{
continue;
}
//Buoyancy
b2Vec2 buoyancyForce = gravity.GetNegative();
buoyancyForce.Multiply(density * area);
body.ApplyForce(buoyancyForce, massc);
//Linear drag
b2Vec2 dragForce = body.GetLinearVelocityFromWorldPoint(areac);
dragForce.Subtract(velocity);
dragForce.Multiply(-linearDrag * area);
body.ApplyForce(dragForce, areac);
//Angular drag
//TODO: Something that makes more physical sense?
body.ApplyTorque(-body.GetInertia() / body.GetMass() * area * body.GetAngularVelocity() * angularDrag);
}
}
/**
* @inheritDoc
*/
public override float ComputeSubmergedArea(
b2Vec2 normal,
float offset,
b2Transform xf,
b2Vec2 c)
{
// Transform plane into shape co-ordinates
b2Vec2 normalL = b2Math.MulTMV(xf.R, normal);
float offsetL = offset - b2Math.Dot(normal, xf.position);
List <float> depths = new List <float>();
int diveCount = 0;
int intoIndex = -1;
int outoIndex = -1;
bool lastSubmerged = false;
int i;
for (i = 0; i < m_vertexCount; ++i)
{
depths[i] = b2Math.Dot(normalL, m_vertices[i]) - offsetL;
bool isSubmerged = depths[i] < -float.MinValue;
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 = new b2MassData();
ComputeMass(md, 1);
c.SetV(b2Math.MulX(xf, md.center));
return(md.mass);
}
else
{
//Completely dry
return(0);
}
break;
case 1:
if (intoIndex == -1)
{
intoIndex = m_vertexCount - 1;
}
else
{
outoIndex = m_vertexCount - 1;
}
break;
}
int intoIndex2 = (intoIndex + 1) % m_vertexCount;
int outoIndex2 = (outoIndex + 1) % m_vertexCount;
float intoLamdda = (0.0f - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float outoLamdda = (0.0f - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
b2Vec2 intoVec = new b2Vec2(m_vertices[intoIndex].x * (1.0f - intoLamdda) + m_vertices[intoIndex2].x * intoLamdda,
m_vertices[intoIndex].y * (1.0f - intoLamdda) + m_vertices[intoIndex2].y * intoLamdda);
b2Vec2 outoVec = new b2Vec2(m_vertices[outoIndex].x * (1.0f - outoLamdda) + m_vertices[outoIndex2].x * outoLamdda,
m_vertices[outoIndex].y * (1.0f - outoLamdda) + m_vertices[outoIndex2].y * outoLamdda);
// Initialize accumulator
float area = 0.0f;
b2Vec2 center = new b2Vec2();
b2Vec2 p2 = m_vertices[intoIndex2];
b2Vec2 p3;
// 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];
}
float triangleArea = 0.5f * ((p2.x - intoVec.x) * (p3.y - intoVec.y) - (p2.y - intoVec.y) * (p3.x - intoVec.x));
area += triangleArea;
// Area weighted centroid
center.x += triangleArea * (intoVec.x + p2.x + p3.x) / 3.0f;
center.y += triangleArea * (intoVec.y + p2.y + p3.y) / 3.0f;
p2 = p3;
}
//Normalize and transform centroid
center.Multiply(1.0f / area);
c.SetV(b2Math.MulX(xf, center));
return(area);
}
public override void Step(b2TimeStep step)
{
//Inlined
b2ControllerEdge i = null;
b2Body body1 = null;
b2Vec2 p1 = null;
float mass1 = 0.0f;
b2ControllerEdge j = null;
b2Body body2 = null;
b2Vec2 p2 = null;
float dx = 0.0f;
float dy = 0.0f;
float r2 = 0.0f;
b2Vec2 f = null;
if (invSqr)
{
for (i = m_bodyList; i != null; i = i.nextBody)
{
body1 = i.body;
p1 = body1.GetWorldCenter();
mass1 = body1.GetMass();
for (j = m_bodyList; j != i; j = j.nextBody)
{
body2 = j.body;
p2 = body2.GetWorldCenter();
dx = p2.x - p1.x;
dy = p2.y - p1.y;
r2 = dx * dx + dy * dy;
if (r2 < float.MinValue)
{
continue;
}
f = new b2Vec2(dx, dy);
f.Multiply(G / r2 / Mathf.Sqrt(r2) * mass1 * body2.GetMass());
if (body1.IsAwake())
{
body1.ApplyForce(f, p1);
}
f.Multiply(-1.0f);
if (body2.IsAwake())
{
body2.ApplyForce(f, p2);
}
}
}
}
else
{
for (i = m_bodyList; i != null; i = i.nextBody)
{
body1 = i.body;
p1 = body1.GetWorldCenter();
mass1 = body1.GetMass();
for (j = m_bodyList; j != i; j = j.nextBody)
{
body2 = j.body;
p2 = body2.GetWorldCenter();
dx = p2.x - p1.x;
dy = p2.y - p1.y;
r2 = dx * dx + dy * dy;
if (r2 < float.MinValue)
{
continue;
}
f = new b2Vec2(dx, dy);
f.Multiply(G / r2 * mass1 * body2.GetMass());
if (body1.IsAwake())
{
body1.ApplyForce(f, p1);
}
f.Multiply(-1);
if (body2.IsAwake())
{
body2.ApplyForce(f, p2);
}
}
}
}
}
