public override void accumulateInternalForces() { base.accumulateInternalForces(); // internal spring forces. Vector2 force = new Vector2(); for (int i = 0; i < mSprings.Count; i++) { InternalSpring s = mSprings[i]; VectorTools.calculateSpringForce(ref mPointMasses[s.pointMassA].Position, ref mPointMasses[s.pointMassA].Velocity, ref mPointMasses[s.pointMassB].Position, ref mPointMasses[s.pointMassB].Velocity, s.springD, s.springK, s.damping, ref force); mPointMasses[s.pointMassA].Force.x += force.x; mPointMasses[s.pointMassA].Force.y += force.y; mPointMasses[s.pointMassB].Force.x -= force.x; mPointMasses[s.pointMassB].Force.y -= force.y; } // shape matching forces. if (mShapeMatchingOn) { Vector2 p = DerivedPos; mBaseShape.transformVertices(ref p, DerivedAngle, ref mScale, ref mGlobalShape); DerivedPos = p; for (int i = 0; i < mPointMasses.Count; i++) { if (mShapeSpringK > 0) { if (!mKinematic) { VectorTools.calculateSpringForce(ref mPointMasses[i].Position, ref mPointMasses[i].Velocity, ref mGlobalShape[i], ref mPointMasses[i].Velocity, 0.0f, mShapeSpringK, mShapeSpringDamp, ref force); } else { Vector2 kinVel = Vector2.zero; VectorTools.calculateSpringForce(ref mPointMasses[i].Position, ref mPointMasses[i].Velocity, ref mGlobalShape[i], ref kinVel, 0.0f, mShapeSpringK, mShapeSpringDamp, ref force); } mPointMasses[i].Force.x += force.x; mPointMasses[i].Force.y += force.y; } } } }
void OnDrawGizmos() { Vector2 p = DerivedPos; mBaseShape.transformVertices(ref p, DerivedAngle, ref mScale, ref mGlobalShape); DerivedPos = p; VertexPositionColor[] shape = new VertexPositionColor[mPointMasses.Count * 2]; VertexPositionColor[] springs = new VertexPositionColor[mSprings.Count * 2]; p = DerivedPos; mBaseShape.transformVertices(ref p, DerivedAngle, ref mScale, ref mGlobalShape); DerivedPos = p; for (int i = 0; i < mPointMasses.Count; i++) { shape[(i * 2) + 0] = new VertexPositionColor(); shape[(i * 2) + 0].Position = VectorTools.vec3FromVec2(mPointMasses[i].Position); shape[(i * 2) + 0].Color = Color.green; shape[(i * 2) + 1] = new VertexPositionColor(); shape[(i * 2) + 1].Position = VectorTools.vec3FromVec2(mGlobalShape[i]); shape[(i * 2) + 1].Color = Color.red; // if(i != 0) // { // // Gizmos.DrawLine(springs[(i * 2) + 0].Position,springs[(i * 2) + 1].Position); // Gizmos.DrawLine(springs[i].Position,springs[i+1].Position); // Gizmos.DrawLine(springs[i+1].Position,springs[i-1].Position); // } // Gizmos.DrawLine(springs[mSprings.Count-1].Position,springs[0].Position); } for (int i = 0; i < mSprings.Count; i++) { springs[(i * 2) + 0] = new VertexPositionColor(); springs[(i * 2) + 0].Position = VectorTools.vec3FromVec2(mPointMasses[mSprings[i].pointMassA].Position); springs[(i * 2) + 0].Color = Color.gray; springs[(i * 2) + 1] = new VertexPositionColor(); springs[(i * 2) + 1].Position = VectorTools.vec3FromVec2(mPointMasses[mSprings[i].pointMassB].Position); springs[(i * 2) + 1].Color = Color.yellow; Gizmos.color = Color.red; if (i != 0) { // Gizmos.DrawLine(springs[(i * 2) + 0].Position,springs[(i * 2) + 1].Position); Gizmos.DrawLine(springs[i * 2].Position, springs[(i * 2) + 1].Position); // Gizmos.DrawLine(springs[(i*2)+1].Position,springs[i-1].Position); } } Gizmos.DrawLine(springs[mSprings.Count - 1].Position, springs[0].Position); }
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; } }
/// <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(); }
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); } } }
void OnDrawGizmos() { Vector2 p = DerivedPos; mBaseShape.transformVertices(ref p, DerivedAngle, ref mScale, ref mGlobalShape); DerivedPos = p; VertexPositionColor[] shape = new VertexPositionColor[mPointMasses.Count]; p = DerivedPos; mBaseShape.transformVertices(ref p, DerivedAngle, ref mScale, ref mGlobalShape); DerivedPos = p; List <Vector2> points = new List <Vector2> (); for (int i = 0; i < mPointMasses.Count; i++) { // shape[i] = new VertexPositionColor(); // shape[i].Position = VectorTools.vec3FromVec2(mPointMasses[i].Position); // shape[i].Color = Color.red; Gizmos.color = Color.white; if (i != 0) { // Gizmos.DrawLine(mPointMasses [i - 1].UnRotatedPsition, mPointMasses [i].UnRotatedPsition); } } for (int i = 0; i < mSprings.Count; i++) { Gizmos.color = Color.red; Gizmos.DrawLine(VectorTools.vec3FromVec2(mPointMasses[mSprings[i].pointMassA].UnRotatedPsition), VectorTools.vec3FromVec2(mPointMasses[mSprings[i].pointMassB].UnRotatedPsition)); } // Gizmos.DrawLine(shape[mPointMasses.Count-1].Position,shape[0].Position); }