/// <summary> /// find the squared distance from a global point in space, to the closest point on a given edge of the body. /// </summary> /// <param name="pt">global point</param> /// <param name="edgeNum">edge to check against. 0 = edge from pt[0] to pt[1], etc.</param> /// <param name="hitPt">returned point on edge in global space</param> /// <param name="normal">returned normal on edge in global space</param> /// <param name="edgeD">returned distance along edge from ptA to ptB [0,1]</param> /// <returns>distance</returns> public float getClosestPointOnEdgeSquared(Vector2 pt, int edgeNum, out Vector2 hitPt, out Vector2 normal, out float edgeD) { hitPt = new Vector2(); hitPt.x = 0f; hitPt.y = 0f; normal = new Vector2(); normal.x = 0f; normal.y = 0f; edgeD = 0f; float dist = 0f; Vector2 ptA = mPointMasses [edgeNum].Position; Vector2 ptB = new Vector2(); if (edgeNum < (mPointMasses.Count - 1)) { ptB = mPointMasses [edgeNum + 1].Position; } else { ptB = mPointMasses [0].Position; } Vector2 toP = new Vector2(); toP.x = pt.x - ptA.x; toP.y = pt.y - ptA.y; Vector2 E = new Vector2(); E.x = ptB.x - ptA.x; E.y = ptB.y - ptA.y; // get the length of the edge, and use that to normalize the vector. float edgeLength = (float)Math.Sqrt((E.x * E.x) + (E.y * E.y)); if (edgeLength > 0.00001f) { E.x /= edgeLength; E.y /= edgeLength; } // normal Vector2 n = new Vector2(); VectorTools.getPerpendicular(ref E, ref n); // calculate the distance! float x; // Vector2.Dot(ref toP, ref E, out x); x = Vector2.Dot(toP, E); if (x <= 0.0f) { // x is outside the line segment, distance is from pt to ptA. //dist = (pt - ptA).Length(); // Vector2.DistanceSquared(ref pt, ref ptA, out dist); dist = Vector2.Distance(pt, ptA); dist = dist * dist; hitPt = ptA; edgeD = 0f; normal = n; } else if (x >= edgeLength) { // x is outside of the line segment, distance is from pt to ptB. //dist = (pt - ptB).Length(); // Vector2.DistanceSquared(ref pt, ref ptB, out dist); dist = Vector2.Distance(pt, ptB); dist = dist * dist; hitPt = ptB; edgeD = 1f; normal = n; } else { // point lies somewhere on the line segment. Vector3 toP3 = new Vector3(); toP3.x = toP.x; toP3.y = toP.y; Vector3 E3 = new Vector3(); E3.x = E.x; E3.y = E.y; //dist = Math.Abs(Vector3.Cross(toP3, E3).z); // Vector3.Cross(ref toP3, ref E3, out E3); E3 = Vector3.Cross(toP3, E3); dist = Mathf.Abs(E3.z * E3.z); hitPt.x = ptA.x + (E.x * x); hitPt.y = ptA.y + (E.y * x); edgeD = x / edgeLength; normal = n; } return(dist); }
private void _handleCollisions() { // handle all collisions! for (int i = 0; i < mCollisionList.Count; i++) { BodyCollisionInfo info = mCollisionList[i]; PointMass A = info.bodyA.getPointMass(info.bodyApm); PointMass B1 = info.bodyB.getPointMass(info.bodyBpmA); PointMass B2 = info.bodyB.getPointMass(info.bodyBpmB); // velocity changes as a result of collision. Vector2 bVel = new Vector2(); bVel.x = (B1.Velocity.x + B2.Velocity.x) * 0.5f; bVel.y = (B1.Velocity.y + B2.Velocity.y) * 0.5f; Vector2 relVel = new Vector2(); relVel.x = A.Velocity.x - bVel.x; relVel.y = A.Velocity.y - bVel.y; float relDot; // Vector2.Dot(ref relVel, ref info.normal, out relDot); relDot = Vector2.Dot(relVel, info.normal); // collision filter! if (!mMaterialPairs[info.bodyA.Material, info.bodyB.Material].CollisionFilter(info.bodyA, info.bodyApm, info.bodyB, info.bodyBpmA, info.bodyBpmB, info.hitPt, relDot)) { continue; } if (info.penetration > mPenetrationThreshold) { //Console.WriteLine("penetration above Penetration Threshold!! penetration={0} threshold={1} difference={2}", // info.penetration, mPenetrationThreshold, info.penetration-mPenetrationThreshold); mPenetrationCount++; continue; } float b1inf = 1.0f - info.edgeD; float b2inf = info.edgeD; float b2MassSum = ((float.IsPositiveInfinity(B1.Mass)) || (float.IsPositiveInfinity(B2.Mass))) ? float.PositiveInfinity : (B1.Mass + B2.Mass); float massSum = A.Mass + b2MassSum; float Amove; float Bmove; if (float.IsPositiveInfinity(A.Mass)) { Amove = 0f; Bmove = (info.penetration) + 0.001f; } else if (float.IsPositiveInfinity(b2MassSum)) { Amove = (info.penetration) + 0.001f; Bmove = 0f; } else { Amove = (info.penetration * (b2MassSum / massSum)); Bmove = (info.penetration * (A.Mass / massSum)); } float B1move = Bmove * b1inf; float B2move = Bmove * b2inf; float AinvMass = (float.IsPositiveInfinity(A.Mass)) ? 0f : 1f / A.Mass; float BinvMass = (float.IsPositiveInfinity(b2MassSum)) ? 0f : 1f / b2MassSum; float jDenom = AinvMass + BinvMass; Vector2 numV = new Vector2(); float elas = 1f + mMaterialPairs[info.bodyA.Material, info.bodyB.Material].Elasticity; numV.x = relVel.x * elas; numV.y = relVel.y * elas; float jNumerator; // Vector2.Dot(ref numV, ref info.normal, out jNumerator); jNumerator = Vector2.Dot(numV, info.normal); jNumerator = -jNumerator; float j = jNumerator / jDenom; if (!float.IsPositiveInfinity(A.Mass)) { A.Position.x += info.normal.x * Amove; A.Position.y += info.normal.y * Amove; } if (!float.IsPositiveInfinity(B1.Mass)) { B1.Position.x -= info.normal.x * B1move; B1.Position.y -= info.normal.y * B1move; } if (!float.IsPositiveInfinity(B2.Mass)) { B2.Position.x -= info.normal.x * B2move; B2.Position.y -= info.normal.y * B2move; } Vector2 tangent = new Vector2(); VectorTools.getPerpendicular(ref info.normal, ref tangent); float friction = mMaterialPairs[info.bodyA.Material, info.bodyB.Material].Friction; float fNumerator; // Vector2.Dot(ref relVel, ref tangent, out fNumerator); fNumerator = Vector2.Dot(relVel, tangent); fNumerator *= friction; float f = fNumerator / jDenom; // adjust velocity if relative velocity is moving toward each other. if (relDot <= 0.0001f) { if (!float.IsPositiveInfinity(A.Mass)) { A.Velocity.x += (info.normal.x * (j / A.Mass)) - (tangent.x * (f / A.Mass)); A.Velocity.y += (info.normal.y * (j / A.Mass)) - (tangent.y * (f / A.Mass)); } if (!float.IsPositiveInfinity(b2MassSum)) { B1.Velocity.x -= (info.normal.x * (j / b2MassSum) * b1inf) - (tangent.x * (f / b2MassSum) * b1inf); B1.Velocity.y -= (info.normal.y * (j / b2MassSum) * b1inf) - (tangent.y * (f / b2MassSum) * b1inf); } if (!float.IsPositiveInfinity(b2MassSum)) { B2.Velocity.x -= (info.normal.x * (j / b2MassSum) * b2inf) - (tangent.x * (f / b2MassSum) * b2inf); B2.Velocity.y -= (info.normal.y * (j / b2MassSum) * b2inf) - (tangent.y * (f / b2MassSum) * b2inf); } } } mCollisionList.Clear(); }