public override void accumulateInternalForces()
{
base.accumulateInternalForces();
// internal forces based on pressure equations. we need 2 loops to do this. one to find the overall volume of the
// body, and 1 to apply forces. we will need the normals for the edges in both loops, so we will cache them and remember them.
mVolume = 0f;
for (int i = 0; i < mPointMasses.Count; i++)
{
int prev = (i > 0) ? i - 1 : mPointMasses.Count - 1;
int next = (i < mPointMasses.Count - 1) ? i + 1 : 0;
// currently we are talking about the edge from i --> j.
// first calculate the volume of the body, and cache normals as we go.
Vector2 edge1N = new Vector2();
edge1N.x = mPointMasses[i].Position.x - mPointMasses[prev].Position.x;
edge1N.y = mPointMasses[i].Position.y - mPointMasses[prev].Position.y;
VectorTools.makePerpendicular(ref edge1N);
Vector2 edge2N = new Vector2();
edge2N.x = mPointMasses[next].Position.x - mPointMasses[i].Position.x;
edge2N.y = mPointMasses[next].Position.y - mPointMasses[i].Position.y;
VectorTools.makePerpendicular(ref edge2N);
Vector2 norm = new Vector2();
norm.x = edge1N.x + edge2N.x;
norm.y = edge1N.y + edge2N.y;
float nL = (float)Math.Sqrt((norm.x * norm.x) + (norm.y * norm.y));
if (nL > 0.001f)
{
norm.x /= nL;
norm.y /= nL;
}
float edgeL = (float)Math.Sqrt((edge2N.x * edge2N.x) + (edge2N.y * edge2N.y));
// cache normal and edge length
mNormalList[i] = norm;
mEdgeLengthList[i] = edgeL;
float xdist = Math.Abs(mPointMasses[i].Position.x - mPointMasses[next].Position.x);
float volumeProduct = xdist * Math.Abs(norm.x) * edgeL;
// add to volume
mVolume += 0.5f * volumeProduct;
}
// now loop through, adding forces!
float invVolume = 1f / mVolume;
for (int i = 0; i < mPointMasses.Count; i++)
{
int j = (i < mPointMasses.Count - 1) ? i + 1 : 0;
float pressureV = (invVolume * mEdgeLengthList[i] * mGasAmount);
mPointMasses[i].Force.x += mNormalList[i].x * pressureV;
mPointMasses[i].Force.y += mNormalList[i].y * pressureV;
mPointMasses[j].Force.x += mNormalList[j].x * pressureV;
mPointMasses[j].Force.y += mNormalList[j].y * pressureV;
}
}
private void bodyCollide(Body bA, Body bB, List <BodyCollisionInfo> infoList)
{
int bApmCount = bA.PointMassCount;
int bBpmCount = bB.PointMassCount;
AABB boxB = bB.getAABB();
// check all PointMasses on bodyA for collision against bodyB. if there is a collision, return detailed info.
BodyCollisionInfo infoAway = new BodyCollisionInfo();
BodyCollisionInfo infoSame = new BodyCollisionInfo();
for (int i = 0; i < bApmCount; i++)
{
Vector2 pt = bA.getPointMass(i).Position;
// early out - if this point is outside the bounding box for bodyB, skip it!
if (!boxB.contains(ref pt))
{
continue;
}
// early out - if this point is not inside bodyB, skip it!
if (!bB.contains(ref pt))
{
continue;
}
int prevPt = (i > 0) ? i - 1 : bApmCount - 1;
int nextPt = (i < bApmCount - 1) ? i + 1 : 0;
Vector2 prev = bA.getPointMass(prevPt).Position;
Vector2 next = bA.getPointMass(nextPt).Position;
// now get the normal for this point. (NOT A UNIT VECTOR)
Vector2 fromPrev = new Vector2();
fromPrev.x = pt.x - prev.x;
fromPrev.y = pt.y - prev.y;
Vector2 toNext = new Vector2();
toNext.x = next.x - pt.x;
toNext.y = next.y - pt.y;
Vector2 ptNorm = new Vector2();
ptNorm.x = fromPrev.x + toNext.x;
ptNorm.y = fromPrev.y + toNext.y;
VectorTools.makePerpendicular(ref ptNorm);
// this point is inside the other body. now check if the edges on either side intersect with and edges on bodyB.
float closestAway = 100000.0f;
float closestSame = 100000.0f;
infoAway.Clear();
infoAway.bodyA = bA;
infoAway.bodyApm = i;
infoAway.bodyB = bB;
infoSame.Clear();
infoSame.bodyA = bA;
infoSame.bodyApm = i;
infoSame.bodyB = bB;
bool found = false;
int b1 = 0;
int b2 = 1;
for (int j = 0; j < bBpmCount; j++)
{
Vector2 hitPt;
Vector2 norm;
float edgeD;
b1 = j;
if (j < bBpmCount - 1)
{
b2 = j + 1;
}
else
{
b2 = 0;
}
Vector2 pt1 = bB.getPointMass(b1).Position;
Vector2 pt2 = bB.getPointMass(b2).Position;
// quick test of distance to each point on the edge, if both are greater than current mins, we can skip!
float distToA = ((pt1.x - pt.x) * (pt1.x - pt.x)) + ((pt1.y - pt.y) * (pt1.y - pt.y));
float distToB = ((pt2.x - pt.x) * (pt2.x - pt.x)) + ((pt2.y - pt.y) * (pt2.y - pt.y));
if ((distToA > closestAway) && (distToA > closestSame) && (distToB > closestAway) && (distToB > closestSame))
{
continue;
}
// test against this edge.
float dist = bB.getClosestPointOnEdgeSquared(pt, j, out hitPt, out norm, out edgeD);
// only perform the check if the normal for this edge is facing AWAY from the point normal.
float dot;
//Vector2.Dot(ref ptNorm, ref edgeNorm, out dot);
// Vector2.Dot(ref ptNorm, ref norm, out dot);
dot = Vector2.Dot(ptNorm, norm);
if (dot <= 0f)
{
if (dist < closestAway)
{
closestAway = dist;
infoAway.bodyBpmA = b1;
infoAway.bodyBpmB = b2;
infoAway.edgeD = edgeD;
infoAway.hitPt = hitPt;
infoAway.normal = norm;
infoAway.penetration = dist;
found = true;
}
}
else
{
if (dist < closestSame)
{
closestSame = dist;
infoSame.bodyBpmA = b1;
infoSame.bodyBpmB = b2;
infoSame.edgeD = edgeD;
infoSame.hitPt = hitPt;
infoSame.normal = norm;
infoSame.penetration = dist;
}
}
}
// we've checked all edges on BodyB. add the collision info to the stack.
if ((found) && (closestAway > mPenetrationThreshold) && (closestSame < closestAway))
{
infoSame.penetration = (float)Math.Sqrt(infoSame.penetration);
infoList.Add(infoSame);
}
else
{
infoAway.penetration = (float)Math.Sqrt(infoAway.penetration);
infoList.Add(infoAway);
}
}
}
