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); } } }