public static void FindIncidentFace(ref Vector2R[] v, Polygon RefPoly, Polygon IncPoly, int referenceIndex)
{
Vector2R referenceNormal = RefPoly.normals[referenceIndex];
// Calculate normal in incident's frame of reference
referenceNormal = RefPoly.u * referenceNormal; // To world space
referenceNormal = IncPoly.u.Transpose() * referenceNormal; // To incident's model space
// Find most anti-normal face on incident polygon
int incidentFace = 0;
double minDot = float.MaxValue;
for (int i = 0; i < IncPoly.vertexCount; ++i)
{
double dot = Vector2R.Dot(referenceNormal, IncPoly.normals[i]);
if (dot < minDot)
{
minDot = dot;
incidentFace = i;
}
}
// Assign face vertices for incidentFace
v[0] = IncPoly.u * IncPoly.vertices[incidentFace] + IncPoly.body.pos;
incidentFace = incidentFace + 1 >= IncPoly.vertexCount ? 0 : incidentFace + 1;
v[1] = IncPoly.u * IncPoly.vertices[incidentFace] + IncPoly.body.pos;
}
// The extreme point along a direction within a polygon
public Vector2R GetSupport(Vector2R dir)
{
double bestProjection = -float.MaxValue; //-FLT_MAX;
Vector2R bestVertex = new Vector2R(0, 0);
for (int i = 0; i < vertexCount; ++i)
{
Vector2R v = vertices[i];
double projection = Vector2R.Dot(v, dir);
if (projection > bestProjection)
{
bestVertex = v;
bestProjection = projection;
}
}
return(bestVertex);
}
public static int Clip(Vector2R n, double c, ref Vector2R[] face)
{
int sp = 0;
Vector2R[] outV = new Vector2R[2] {
face[0],
face[1],
};
// Retrieve distances from each endpoint to the line
// d = ax + by - c
double d1 = Vector2R.Dot(n, face[0]) - c;
double d2 = Vector2R.Dot(n, face[1]) - c;
// If negative (behind plane) clip
if (d1 <= 0.0f)
{
outV[sp++] = face[0];
}
if (d2 <= 0.0f)
{
outV[sp++] = face[1];
}
// If the points are on different sides of the plane
if (d1 * d2 < 0.0f) // less than to ignore -0.0f
{
// Push interesection point
double alpha = d1 / (d1 - d2);
outV[sp] = face[0] + VMath.MultVectDouble((face[1] - face[0]), alpha);
++sp;
}
// Assign our new converted values
face[0] = outV[0];
face[1] = outV[1];
//assert( sp != 3 );
//System.Diagnostics.Debug.Assert(sp != 3);
return(sp);
}
public static double FindAxisLeastPenetration(ref int faceIndex, Polygon A, Polygon B)
{
double bestDistance = -float.MaxValue;
int bestIndex = 0;
for (int i = 0; i < A.vertexCount; ++i)
{
// Retrieve a face normal from A
Vector2R n = A.normals[i];
Vector2R nw = A.u * n;
// Transform face normal into B's model space
Mat22 buT = B.u.Transpose();
n = buT * nw;
// Retrieve support point from B along -n
Vector2R s = B.GetSupport(-n);
// Retrieve vertex on face from A, transform into
// B's model space
Vector2R v = A.vertices[i];
v = A.u * v + A.body.pos;
v -= B.body.pos;
v = buT * v;
// Compute penetration distance (in B's model space)
double d = Vector2R.Dot(n, s - v);
// Store greatest distance
if (d > bestDistance)
{
bestDistance = d;
bestIndex = i;
}
}
faceIndex = bestIndex;
return(bestDistance);
}
public static Vector2R ProjectOnto(this Vector2R source, Vector2R target)
{
return((Vector2R.Dot(source, target) / target.LengthSquared()) * target);
}
public void ApplyImpulse()
{
if (GMath.Equal(a.invmass + b.invmass, 0))
{
InfinitMassCorrection();
return;
}
for (int i = 0; i < contact_count; i++)
{
//calcuate radii from COM to contact
Vector2R ra = contacts[i] - a.pos;
Vector2R rb = contacts[i] - b.pos;
//relative velocity
Vector2R rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
a.velocity - VMath.Cross(a.angularVelocity, ra);
//relative velocity along the normal
double contactVel = Vector2R.Dot(rv, normal);
//do not resolve if velocities are seperating
if (contactVel > 0)
{
return;
}
double raCrossN = VMath.Cross(ra, normal);
double rbCrossN = VMath.Cross(rb, normal);
double invMassSum = a.invmass + b.invmass + (raCrossN * raCrossN) * a.invinertia + (rbCrossN * rbCrossN) * b.invinertia;
//calculate impulse scalar
double j = -(1.0 + e) * contactVel;
j /= invMassSum;
j /= (double)contact_count;
//apply impulse
Vector2R impulse = VMath.MultVectDouble(normal, j); // normal * j;
a.ApplyImpulse(-impulse, ra);
b.ApplyImpulse(impulse, rb);
//friction impulse
rv = b.velocity + VMath.Cross(b.angularVelocity, rb) -
a.velocity - VMath.Cross(a.angularVelocity, ra);
Vector2R t = rv - (normal * Vector2R.Dot(rv, normal));
//t.Normalize();
VMath.NormalizeSafe(ref t);
//j tangent magnitude
double jt = -Vector2R.Dot(rv, t);
jt /= invMassSum;
jt /= (double)contact_count;
//don't apply tiny friction impulses
if (GMath.Equal(jt, 0.0))
{
return;
}
//coulumbs law
Vector2R tangentImpulse;
if (Math.Abs(jt) < j * sf)
{
tangentImpulse = VMath.MultVectDouble(t, df); // t * df;
}
else
{
tangentImpulse = VMath.MultVectDouble(t, -j * df); // t * -j * df
}
//apply friction impulse
a.ApplyImpulse(-tangentImpulse, ra);
b.ApplyImpulse(tangentImpulse, rb);
}
}
public static bool PolygontoPolygonCheck(Collider a, Collider b)
{
Polygon A = (Polygon)a.shape;
Polygon B = (Polygon)b.shape;
//m.contact_count = 0;
// Check for a separating axis with A's face planes
int faceA = 0;
double penetrationA = FindAxisLeastPenetration(ref faceA, A, B);
if (penetrationA >= 0.0f)
{
return(false);
}
// Check for a separating axis with B's face planes
int faceB = 0;
double penetrationB = FindAxisLeastPenetration(ref faceB, B, A);
if (penetrationB >= 0.0f)
{
return(false);
}
int referenceIndex;
//bool flip; // Always point from a to b
Polygon RefPoly; // Reference
Polygon IncPoly; // Incident
// Determine which shape contains reference face
if (GMath.BiasGreaterThan(penetrationA, penetrationB))
{
RefPoly = A;
IncPoly = B;
referenceIndex = faceA;
//flip = false;
}
else
{
RefPoly = B;
IncPoly = A;
referenceIndex = faceB;
//flip = true;
}
// World space incident face
Vector2R[] incidentFace = new Vector2R[2];
FindIncidentFace(ref incidentFace, RefPoly, IncPoly, referenceIndex);
// Setup reference face vertices
Vector2R v1 = RefPoly.vertices[referenceIndex];
referenceIndex = referenceIndex + 1 == RefPoly.vertexCount ? 0 : referenceIndex + 1;
Vector2R v2 = RefPoly.vertices[referenceIndex];
// Transform vertices to world space
v1 = RefPoly.u * v1 + RefPoly.body.pos;
v2 = RefPoly.u * v2 + RefPoly.body.pos;
// Calculate reference face side normal in world space
Vector2R sidePlaneNormal = (v2 - v1);
VMath.NormalizeSafe(ref sidePlaneNormal);
// Orthogonalize
Vector2R refFaceNormal = new Vector2R(sidePlaneNormal.Y, -sidePlaneNormal.X);
// ax + by = c
// c is distance from origin
double refC = Vector2R.Dot(refFaceNormal, v1);
double negSide = -Vector2R.Dot(sidePlaneNormal, v1);
double posSide = Vector2R.Dot(sidePlaneNormal, v2);
// Clip incident face to reference face side planes
if (Clip(-sidePlaneNormal, negSide, ref incidentFace) < 2)
{
return(false); // Due to floating point error, possible to not have required points
}
if (Clip(sidePlaneNormal, posSide, ref incidentFace) < 2)
{
return(false); // Due to floating point error, possible to not have required points
}
// Flip
//m.normal = flip ? -refFaceNormal : refFaceNormal;
// Keep points behind reference face
int cp = 0; // clipped points behind reference face
double separation = Vector2R.Dot(refFaceNormal, incidentFace[0]) - refC;
if (separation <= 0.0f)
{
//m.contacts[cp] = incidentFace[0];
//m.penetration = -separation;
++cp;
}
//else
// m.penetration = 0;
separation = Vector2R.Dot(refFaceNormal, incidentFace[1]) - refC;
if (separation <= 0.0f)
{
//m.contacts[cp] = incidentFace[1];
//
//m.penetration += -separation;
++cp;
// Average penetration
//m.penetration /= (double)cp;
}
//m.contact_count = cp;
return(cp > 0);
}
public static bool CircletoPolygonCheck(Collider a, Collider b)
{
Circle A = (Circle)a.shape;
Polygon B = (Polygon)b.shape;
//m.contact_count = 0;
// Transform circle center to Polygon model space
Vector2R center = a.pos;
center = B.u.Transpose() * (center - b.pos);
// Find edge with minimum penetration
// Exact concept as using support points in Polygon vs Polygon
double separation = -float.MaxValue;
int faceNormal = 0;
for (int i = 0; i < B.vertexCount; ++i)
{
double s = Vector2R.Dot(B.normals[i], center - B.vertices[i]);
if (s > A.radius)
{
return(false);
}
if (s > separation)
{
separation = s;
faceNormal = i;
}
}
// Grab face's vertices
Vector2R v1 = B.vertices[faceNormal];
int i2 = faceNormal + 1 < B.vertexCount ? faceNormal + 1 : 0;
Vector2R v2 = B.vertices[i2];
// Check to see if center is within polygon
if (separation < GMath.EPSILON)
{
//m.contact_count = 1;
//m.normal = -(B.u * B.normals[faceNormal]);
//m.contacts[0] = VMath.MultVectDouble(m.normal, A.radius) + a.pos;
//m.penetration = A.radius;
return(true);
}
// Determine which voronoi region of the edge center of circle lies within
double dot1 = Vector2R.Dot(center - v1, v2 - v1);
double dot2 = Vector2R.Dot(center - v2, v1 - v2);
//m.penetration = A.radius - separation;
// Closest to v1
if (dot1 <= 0.0f)
{
if (Vector2R.DistanceSquared(center, v1) > A.radius * A.radius)
{
return(false);
}
//m.contact_count = 1;
//Vector2 n = v1 - center;
//n = B.u * n;
//n.Normalize();
//m.normal = n;
//v1 = B.u * v1 + b.pos;
//m.contacts[0] = v1;
}
// Closest to v2
else if (dot2 <= 0.0f)
{
if (Vector2R.DistanceSquared(center, v2) > A.radius * A.radius)
{
return(false);
}
//m.contact_count = 1;
//Vector2 n = v2 - center;
//v2 = B.u * v2 + b.pos;
//m.contacts[0] = v2;
//n = B.u * n;
//n.Normalize();
//m.normal = n;
}
// Closest to face
else
{
Vector2R n = B.normals[faceNormal];
if (Vector2R.Dot(center - v1, n) > A.radius)
{
return(false);
}
//n = B.u * n;
//m.normal = -n;
//m.contacts[0] = VMath.MultVectDouble(m.normal, A.radius) + a.pos;
//m.contact_count = 1;
}
return(true);
}
public override void PlayerControl(Input input)
{
Vector2R newstickpos = input.GetRightStick(shovelReach, true).toV2R(); //input.GetRightStick();
Vector2R pos = newstickpos * shovelReach;
Vector2R worldStickPos = parent.body.pos + pos;
Vector2R diff = worldStickPos - shovelNode.body.pos;
//float angle = Utils.VectorToAngle(shovelNode.body.pos - parent.body.pos) + VMath.PIbyTwo % VMath.twoPI;
Vector2R shovelDir = shovelNode.body.pos - parent.body.pos;
shovelDir = new Vector2R(shovelDir.Y, -shovelDir.X);
shovelNode.body.SetOrientV2(shovelDir);
if (modeShovelPosition == ModeShovelPosition.AbsoluteStickPos)
{
shovelNode.body.pos = worldStickPos;
}
else if (modeShovelPosition == ModeShovelPosition.PhysicsBased)
{
float len = diff.Length();
if (len < 1)
{
shovelNode.body.velocity = Vector2R.Zero;
}
else
{
float velLen = shovelNode.body.velocity.Length();
Vector2R diffcopy = diff;
VMath.NormalizeSafe(ref diffcopy);
Vector2R normalizedVel = shovelNode.body.velocity;
VMath.NormalizeSafe(ref normalizedVel);
float result = 0;
Vector2R.Dot(ref diffcopy, ref normalizedVel, out result);
diffcopy *= result;
Vector2R force = (diff / physicsDivisor);
if (shovelling && compoundedMass >= 1)
{
force /= compoundedMass * 1;
}
shovelNode.body.velocity = diffcopy + force;
//shovelNode.body.ApplyForce(force);
}
}
if (shovelling)
{
//if (fc.newGamePadState.Triggers.Right < deadzone && fc.oldGamePadState.Triggers.Right > deadzone)
if (input.BtnReleased(InputButtons.RightTrigger_Mouse1))
{
shovelling = false;
foreach (Node n in shovelLink.targets.ToList())
{
if (physicsThrow)
{
n.body.velocity = n.body.effvelocity;
}
else
{
Vector2R stickdirection = newstickpos;
VMath.NormalizeSafe(ref stickdirection);
n.body.velocity = stickdirection * throwSpeed;
}
n.collision.active = true;
shovelLink.targets.Remove(n);
n.body.ClearExclusionChecks();
n.body.color = n.body.permaColor;
}
shovelLink.formation.UpdateFormation();
shovelLink.active = false;
shovelNode.room.AllActiveLinks.Remove(shovelLink);
compoundedMass = 0f;
}
}
else
{
if (input.BtnClicked(InputButtons.RightTrigger_Mouse1))
{
shovelling = true;
ObservableHashSet <Node> capturedNodes = new ObservableHashSet <Node>();
int count = 0;
Action <Collider, Collider> del = delegate(Collider c1, Collider c2) {
if (count >= maxShovelCapacity)
{
return;
}
if (c2.parent.dataStore.ContainsKey("shovelnodeparent"))
{
return;
}
if (c2.parent.HasComp <Diode>())
{
return;
}
if (modePlayers != ModePlayers.GrabBoth && c2.parent.IsPlayer)
{
if (modePlayers == ModePlayers.GrabNone)
{
return;
}
if (modePlayers == ModePlayers.GrabSelf && c2.parent != parent)
{
return;
}
if (modePlayers == ModePlayers.GrabOtherPlayers && c2.parent == parent)
{
return;
}
}
float dist = Vector2R.Distance(c1.pos, c2.pos);
if (dist <= scoopReach)
{
count++;
capturedNodes.Add(c2.parent);
c2.parent.body.color = parent.body.color;
}
};
shovelNode.room.GridsystemAffect.retrieveOffsetArraysAffect(shovelNode.body, del, scoopReach * 2);
shovelLink.targets = capturedNodes;
shovelLink.formation.UpdateFormation();
shovelLink.active = true;
shovelNode.room.AllActiveLinks.Add(shovelLink);
compoundedMass = 0f;
foreach (Node n in capturedNodes)
{
n.collision.active = false;
compoundedMass += n.body.mass;
}
}
}
}
