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