private void updateBodyBitmask(Body body) { AABB box = body.getAABB(); int minX = (int)Math.Floor((box.Min.x - mWorldLimits.Min.x) / mWorldGridStep.x); int maxX = (int)Math.Floor((box.Max.x - mWorldLimits.Min.x) / mWorldGridStep.x); if (minX < 0) { minX = 0; } else if (minX > 32) { minX = 32; } if (maxX < 0) { maxX = 0; } else if (maxX > 32) { maxX = 32; } int minY = (int)Math.Floor((box.Min.y - mWorldLimits.Min.y) / mWorldGridStep.y); int maxY = (int)Math.Floor((box.Max.y - mWorldLimits.Min.y) / mWorldGridStep.y); if (minY < 0) { minY = 0; } else if (minY > 32) { minY = 32; } if (maxY < 0) { maxY = 0; } else if (maxY > 32) { maxY = 32; } body.mBitMaskX.clear(); for (int i = minX; i <= maxX; i++) { body.mBitMaskX.setOn(i); } body.mBitMaskY.clear(); for (int i = minY; i <= maxY; i++) { body.mBitMaskY.setOn(i); } //Console.WriteLine("Body bitmask: minX{0} maxX{1} minY{2} maxY{3}", minX, maxX, minY, minY, maxY); }
private void updateBodyBitmask(Body body) { AABB box = body.getAABB(); int minX = (int)Math.Floor((box.Min.X - mWorldLimits.Min.X) / mWorldGridStep.X); int maxX = (int)Math.Floor((box.Max.X - mWorldLimits.Min.X) / mWorldGridStep.X); if (minX < 0) { minX = 0; } else if (minX > 32) { minX = 32; } if (maxX < 0) { maxX = 0; } else if (maxX > 32) { maxX = 32; } int minY = (int)Math.Floor((box.Min.Y - mWorldLimits.Min.Y) / mWorldGridStep.Y); int maxY = (int)Math.Floor((box.Max.Y - mWorldLimits.Min.Y) / mWorldGridStep.Y); if (minY < 0) { minY = 0; } else if (minY > 32) { minY = 32; } if (maxY < 0) { maxY = 0; } else if (maxY > 32) { maxY = 32; } body.mBitMaskX.clear(); for (int i = minX; i <= maxX; i++) body.mBitMaskX.setOn(i); body.mBitMaskY.clear(); for (int i = minY; i <= maxY; i++) body.mBitMaskY.setOn(i); //Console.WriteLine("Body bitmask: minX{0} maxX{1} minY{2} maxY{3}", minX, maxX, minY, minY, maxY); }
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); 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); } } }
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); } } }