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