public void Normalize() { var v = new UnityEngine.Vector2(this.x, this.y); v.Normalize(); this.x = v.x; this.y = v.y; }
public PolyNode GetRightestConnection(PolyNode incoming) { if (NConnected == 0) { Debug.Assert(false); // This means the connection graph is inconsistent } if (NConnected == 1) { //b2Assert(false); // Because of the possibility of collapsing nearby points, // we may end up with "spider legs" dangling off of a region. // The correct behavior here is to turn around. return(incoming); } Vector2 inDir = Position - incoming.Position; float inLength = inDir.magnitude; inDir.Normalize(); Debug.Assert(inLength > Settings.Epsilon); PolyNode result = null; for (int i = 0; i < NConnected; ++i) { if (Connected[i] == incoming) { continue; } Vector2 testDir = Connected[i].Position - Position; float testLengthSqr = testDir.sqrMagnitude; testDir.Normalize(); Debug.Assert(testLengthSqr >= Settings.Epsilon * Settings.Epsilon); float myCos = Vector2.Dot(inDir, testDir); float mySin = MathUtils.Cross(inDir, testDir); if (result != null) { Vector2 resultDir = result.Position - Position; resultDir.Normalize(); float resCos = Vector2.Dot(inDir, resultDir); float resSin = MathUtils.Cross(inDir, resultDir); if (IsRighter(mySin, myCos, resSin, resCos)) { result = Connected[i]; } } else { result = Connected[i]; } } Debug.Assert(result != null); return(result); }
internal override void SolveVelocityConstraints(ref TimeStep step) { Body bA = BodyA; Vector2 vA = bA.LinearVelocityInternal; float wA = bA.AngularVelocityInternal; float mA = bA.InvMass; float iA = bA.InvI; Transform xfA; bA.GetTransform(out xfA); Vector2 rA = MathUtils.Multiply(ref xfA.R, LocalAnchorA - bA.LocalCenter); // Solve angular friction { float Cdot = -wA; float impulse = -_angularMass * Cdot; float oldImpulse = _angularImpulse; float maxImpulse = step.dt * MaxTorque; _angularImpulse = MathUtils.Clamp(_angularImpulse + impulse, -maxImpulse, maxImpulse); impulse = _angularImpulse - oldImpulse; wA -= iA * impulse; } // Solve linear friction { Vector2 Cdot = -vA - MathUtils.Cross(wA, rA); Vector2 impulse = -MathUtils.Multiply(ref _linearMass, Cdot); Vector2 oldImpulse = _linearImpulse; _linearImpulse += impulse; float maxImpulse = step.dt * MaxForce; if (_linearImpulse.sqrMagnitude > maxImpulse * maxImpulse) { _linearImpulse.Normalize(); _linearImpulse *= maxImpulse; } impulse = _linearImpulse - oldImpulse; vA -= mA * impulse; wA -= iA * MathUtils.Cross(rA, impulse); } bA.LinearVelocityInternal = vA; bA.AngularVelocityInternal = wA; }
/// <summary> /// Cast a ray against a child shape. /// </summary> /// <param name="output">The ray-cast results.</param> /// <param name="input">The ray-cast input parameters.</param> /// <param name="transform">The transform to be applied to the shape.</param> /// <param name="childIndex">The child shape index.</param> /// <returns>True if the ray-cast hits the shape</returns> public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { // Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius output = new RayCastOutput(); Vector2 position = transform.Position + MathUtils.Multiply(ref transform.R, Position); Vector2 s = input.Point1 - position; float b = Vector2.Dot(s, s) - Radius * Radius; // Solve quadratic equation. Vector2 r = input.Point2 - input.Point1; float c = Vector2.Dot(s, r); float rr = Vector2.Dot(r, r); float sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < Settings.Epsilon) { return(false); } // Find the point of intersection of the line with the circle. float a = -(c + (float)Math.Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.MaxFraction * rr) { a /= rr; output.Fraction = a; Vector2 norm = (s + a * r); norm.Normalize(); output.Normal = norm; return(true); } return(false); }
internal override bool SolvePositionConstraints() { Body bA = BodyA; Body bB = BodyB; Transform xf1; bA.GetTransform(out xf1); Transform xf2; bB.GetTransform(out xf2); Vector2 rA = MathUtils.Multiply(ref xf1.R, LocalAnchorA - bA.LocalCenter); Vector2 rB = MathUtils.Multiply(ref xf2.R, LocalAnchorB - bB.LocalCenter); Vector2 u = bB.Sweep.C + rB - bA.Sweep.C - rA; float length = u.magnitude; u.Normalize(); float C = length - MaxLength; C = MathUtils.Clamp(C, 0.0f, Settings.MaxLinearCorrection); float impulse = -_mass * C; Vector2 P = impulse * u; bA.Sweep.C -= bA.InvMass * P; bA.Sweep.A -= bA.InvI * MathUtils.Cross(rA, P); bB.Sweep.C += bB.InvMass * P; bB.Sweep.A += bB.InvI * MathUtils.Cross(rB, P); bA.SynchronizeTransform(); bB.SynchronizeTransform(); return(length - MaxLength < Settings.LinearSlop); }
protected override void ValueUpdate() { if (_radialValueUpdate) { // TODO Remove unityengine code var mpos = Utils.MousePosition(); UnityEngine.Vector2 pos = Utils.MousePosition() - Center; pos.Normalize(); // get var zeroOne = new UnityEngine.Vector2(0, 1); float angle01 = 360 - UnityEngine.Vector2.Angle(zeroOne, pos); if (mpos.x < Center.x) { angle01 = 180 - (angle01 - 180); } Value = Utils.Map(angle01, 0, 360, MinRange, MaxRange); } else { base.ValueUpdate(); } }
public static void ComputeDistance(out DistanceOutput output, out SimplexCache cache, DistanceInput input) { cache = new SimplexCache(); ++GJKCalls; // Initialize the simplex. Simplex simplex = new Simplex(); simplex.ReadCache(ref cache, input.ProxyA, ref input.TransformA, input.ProxyB, ref input.TransformB); // Get simplex vertices as an array. const int k_maxIters = 20; // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. FixedArray3 <int> saveA = new FixedArray3 <int>(); FixedArray3 <int> saveB = new FixedArray3 <int>(); Vector2 closestPoint = simplex.GetClosestPoint(); float distanceSqr1 = closestPoint.sqrMagnitude; float distanceSqr2 = distanceSqr1; // Main iteration loop. int iter = 0; while (iter < k_maxIters) { // Copy simplex so we can identify duplicates. int saveCount = simplex.Count; for (int i = 0; i < saveCount; ++i) { saveA[i] = simplex.V[i].IndexA; saveB[i] = simplex.V[i].IndexB; } switch (simplex.Count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: Debug.Assert(false); break; } // If we have 3 points, then the origin is in the corresponding triangle. if (simplex.Count == 3) { break; } // Compute closest point. Vector2 p = simplex.GetClosestPoint(); distanceSqr2 = p.sqrMagnitude; // Ensure progress if (distanceSqr2 >= distanceSqr1) { //break; } distanceSqr1 = distanceSqr2; // Get search direction. Vector2 d = simplex.GetSearchDirection(); // Ensure the search direction is numerically fit. if (d.sqrMagnitude < Settings.Epsilon * Settings.Epsilon) { // The origin is probably contained by a line segment // or triangle. Thus the shapes are overlapped. // We can't return zero here even though there may be overlap. // In case the simplex is a point, segment, or triangle it is difficult // to determine if the origin is contained in the CSO or very close to it. break; } // Compute a tentative new simplex vertex using support points. SimplexVertex vertex = simplex.V[simplex.Count]; vertex.IndexA = input.ProxyA.GetSupport(MathUtils.MultiplyT(ref input.TransformA.R, -d)); vertex.WA = MathUtils.Multiply(ref input.TransformA, input.ProxyA.Vertices[vertex.IndexA]); vertex.IndexB = input.ProxyB.GetSupport(MathUtils.MultiplyT(ref input.TransformB.R, d)); vertex.WB = MathUtils.Multiply(ref input.TransformB, input.ProxyB.Vertices[vertex.IndexB]); vertex.W = vertex.WB - vertex.WA; simplex.V[simplex.Count] = vertex; // Iteration count is equated to the number of support point calls. ++iter; ++GJKIters; // Check for duplicate support points. This is the main termination criteria. bool duplicate = false; for (int i = 0; i < saveCount; ++i) { if (vertex.IndexA == saveA[i] && vertex.IndexB == saveB[i]) { duplicate = true; break; } } // If we found a duplicate support point we must exit to avoid cycling. if (duplicate) { break; } // New vertex is ok and needed. ++simplex.Count; } GJKMaxIters = Math.Max(GJKMaxIters, iter); // Prepare output. simplex.GetWitnessPoints(out output.PointA, out output.PointB); output.Distance = (output.PointA - output.PointB).magnitude; output.Iterations = iter; // Cache the simplex. simplex.WriteCache(ref cache); // Apply radii if requested. if (input.UseRadii) { float rA = input.ProxyA.Radius; float rB = input.ProxyB.Radius; if (output.Distance > rA + rB && output.Distance > Settings.Epsilon) { // Shapes are still no overlapped. // Move the witness points to the outer surface. output.Distance -= rA + rB; Vector2 normal = output.PointB - output.PointA; normal.Normalize(); output.PointA += rA * normal; output.PointB -= rB * normal; } else { // Shapes are overlapped when radii are considered. // Move the witness points to the middle. Vector2 p = 0.5f * (output.PointA + output.PointB); output.PointA = p; output.PointB = p; output.Distance = 0.0f; } } }
/// <summary> /// Triangulates a polygon using simple ear-clipping algorithm. Returns /// size of Triangle array unless the polygon can't be triangulated. /// This should only happen if the polygon self-intersects, /// though it will not _always_ return null for a bad polygon - it is the /// caller's responsibility to check for self-intersection, and if it /// doesn't, it should at least check that the return value is non-null /// before using. You're warned! /// /// Triangles may be degenerate, especially if you have identical points /// in the input to the algorithm. Check this before you use them. /// /// This is totally unoptimized, so for large polygons it should not be part /// of the simulation loop. /// /// Warning: Only works on simple polygons. /// </summary> /// <returns></returns> public static List<Triangle> TriangulatePolygon(Vertices vertices) { List<Triangle> results = new List<Triangle>(); if (vertices.Count < 3) return new List<Triangle>(); //Recurse and split on pinch points Vertices pA, pB; Vertices pin = new Vertices(vertices); if (ResolvePinchPoint(pin, out pA, out pB)) { List<Triangle> mergeA = TriangulatePolygon(pA); List<Triangle> mergeB = TriangulatePolygon(pB); if (mergeA.Count == -1 || mergeB.Count == -1) throw new Exception("Can't triangulate your polygon."); for (int i = 0; i < mergeA.Count; ++i) { results.Add(new Triangle(mergeA[i])); } for (int i = 0; i < mergeB.Count; ++i) { results.Add(new Triangle(mergeB[i])); } return results; } Triangle[] buffer = new Triangle[vertices.Count - 2]; int bufferSize = 0; float[] xrem = new float[vertices.Count]; float[] yrem = new float[vertices.Count]; for (int i = 0; i < vertices.Count; ++i) { xrem[i] = vertices[i].x; yrem[i] = vertices[i].y; } int vNum = vertices.Count; while (vNum > 3) { // Find an ear int earIndex = -1; float earMaxMinCross = -10.0f; for (int i = 0; i < vNum; ++i) { if (IsEar(i, xrem, yrem, vNum)) { int lower = Remainder(i - 1, vNum); int upper = Remainder(i + 1, vNum); Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]); Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]); Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]); d1.Normalize(); d2.Normalize(); d3.Normalize(); float cross12; MathUtils.Cross(ref d1, ref d2, out cross12); cross12 = Math.Abs(cross12); float cross23; MathUtils.Cross(ref d2, ref d3, out cross23); cross23 = Math.Abs(cross23); float cross31; MathUtils.Cross(ref d3, ref d1, out cross31); cross31 = Math.Abs(cross31); //Find the maximum minimum angle float minCross = Math.Min(cross12, Math.Min(cross23, cross31)); if (minCross > earMaxMinCross) { earIndex = i; earMaxMinCross = minCross; } } } // If we still haven't found an ear, we're screwed. // Note: sometimes this is happening because the // remaining points are collinear. Really these // should just be thrown out without halting triangulation. if (earIndex == -1) { for (int i = 0; i < bufferSize; i++) { results.Add(new Triangle(buffer[i])); } return results; } // Clip off the ear: // - remove the ear tip from the list --vNum; float[] newx = new float[vNum]; float[] newy = new float[vNum]; int currDest = 0; for (int i = 0; i < vNum; ++i) { if (currDest == earIndex) ++currDest; newx[i] = xrem[currDest]; newy[i] = yrem[currDest]; ++currDest; } // - add the clipped triangle to the triangle list int under = (earIndex == 0) ? (vNum) : (earIndex - 1); int over = (earIndex == vNum) ? 0 : (earIndex + 1); Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under], yrem[under]); buffer[bufferSize] = toAdd; ++bufferSize; // - replace the old list with the new one xrem = newx; yrem = newy; } Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]); buffer[bufferSize] = tooAdd; ++bufferSize; for (int i = 0; i < bufferSize; i++) { results.Add(new Triangle(buffer[i])); } return results; }
/// <summary> /// Cast a ray against a child shape. /// </summary> /// <param name="output">The ray-cast results.</param> /// <param name="input">The ray-cast input parameters.</param> /// <param name="transform">The transform to be applied to the shape.</param> /// <param name="childIndex">The child shape index.</param> /// <returns>True if the ray-cast hits the shape</returns> public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { // p = p1 + t * d // v = v1 + s * e // p1 + t * d = v1 + s * e // s * e - t * d = p1 - v1 output = new RayCastOutput(); // Put the ray into the edge's frame of reference. Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position); Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position); Vector2 d = p2 - p1; Vector2 v1 = _vertex1; Vector2 v2 = _vertex2; Vector2 e = v2 - v1; Vector2 normal = new Vector2(e.y, -e.x); normal.Normalize(); // q = p1 + t * d // dot(normal, q - v1) = 0 // dot(normal, p1 - v1) + t * dot(normal, d) = 0 float numerator = Vector2.Dot(normal, v1 - p1); float denominator = Vector2.Dot(normal, d); if (denominator == 0.0f) { return(false); } float t = numerator / denominator; if (t < 0.0f || 1.0f < t) { return(false); } Vector2 q = p1 + t * d; // q = v1 + s * r // s = dot(q - v1, r) / dot(r, r) Vector2 r = v2 - v1; float rr = Vector2.Dot(r, r); if (rr == 0.0f) { return(false); } float s = Vector2.Dot(q - v1, r) / rr; if (s < 0.0f || 1.0f < s) { return(false); } output.Fraction = t; if (numerator > 0.0f) { output.Normal = -normal; } else { output.Normal = normal; } return(true); }
/// <summary> /// Cast a ray against a child shape. /// </summary> /// <param name="output">The ray-cast results.</param> /// <param name="input">The ray-cast input parameters.</param> /// <param name="transform">The transform to be applied to the shape.</param> /// <param name="childIndex">The child shape index.</param> /// <returns>True if the ray-cast hits the shape</returns> public override bool RayCast(out RayCastOutput output, ref RayCastInput input, ref Transform transform, int childIndex) { // p = p1 + t * d // v = v1 + s * e // p1 + t * d = v1 + s * e // s * e - t * d = p1 - v1 output = new RayCastOutput(); // Put the ray into the edge's frame of reference. Vector2 p1 = MathUtils.MultiplyT(ref transform.R, input.Point1 - transform.Position); Vector2 p2 = MathUtils.MultiplyT(ref transform.R, input.Point2 - transform.Position); Vector2 d = p2 - p1; Vector2 v1 = _vertex1; Vector2 v2 = _vertex2; Vector2 e = v2 - v1; Vector2 normal = new Vector2(e.y, -e.x); normal.Normalize(); // q = p1 + t * d // dot(normal, q - v1) = 0 // dot(normal, p1 - v1) + t * dot(normal, d) = 0 float numerator = Vector2.Dot(normal, v1 - p1); float denominator = Vector2.Dot(normal, d); if (denominator == 0.0f) { return false; } float t = numerator / denominator; if (t < 0.0f || 1.0f < t) { return false; } Vector2 q = p1 + t * d; // q = v1 + s * r // s = dot(q - v1, r) / dot(r, r) Vector2 r = v2 - v1; float rr = Vector2.Dot(r, r); if (rr == 0.0f) { return false; } float s = Vector2.Dot(q - v1, r) / rr; if (s < 0.0f || 1.0f < s) { return false; } output.Fraction = t; if (numerator > 0.0f) { output.Normal = -normal; } else { output.Normal = normal; } return true; }
/// <summary> /// Copy vertices. This assumes the vertices define a convex polygon. /// It is assumed that the exterior is the the right of each edge. /// </summary> /// <param name="vertices">The vertices.</param> public void Set(Vertices vertices) { Debug.Assert(vertices.Count >= 3 && vertices.Count <= Settings.MaxPolygonVertices); #pragma warning disable 0162 if (Settings.ConserveMemory) Vertices = vertices; else // Copy vertices. Vertices = new Vertices(vertices); #pragma warning restore Normals = new Vertices(vertices.Count); // Compute normals. Ensure the edges have non-zero length. for (int i = 0; i < vertices.Count; ++i) { int i1 = i; int i2 = i + 1 < vertices.Count ? i + 1 : 0; Vector2 edge = Vertices[i2] - Vertices[i1]; Debug.Assert(edge.sqrMagnitude > Settings.Epsilon * Settings.Epsilon); Vector2 temp = new Vector2(edge.y, -edge.x); temp.Normalize(); Normals.Add(temp); } #if DEBUG // Ensure the polygon is convex and the interior // is to the left of each edge. for (int i = 0; i < Vertices.Count; ++i) { int i1 = i; int i2 = i + 1 < Vertices.Count ? i + 1 : 0; Vector2 edge = Vertices[i2] - Vertices[i1]; for (int j = 0; j < vertices.Count; ++j) { // Don't check vertices on the current edge. if (j == i1 || j == i2) { continue; } Vector2 r = Vertices[j] - Vertices[i1]; // Your polygon is non-convex (it has an indentation) or // has colinear edges. float s = edge.x * r.y - edge.y * r.x; Debug.Assert(s > 0.0f); } } #endif // Compute the polygon mass data ComputeProperties(); }
/// <summary> /// Copy vertices. This assumes the vertices define a convex polygon. /// It is assumed that the exterior is the the right of each edge. /// </summary> /// <param name="vertices">The vertices.</param> public void Set(Vertices vertices) { Debug.Assert(vertices.Count >= 3 && vertices.Count <= Settings.MaxPolygonVertices); #pragma warning disable 0162 if (Settings.ConserveMemory) { Vertices = vertices; } else { // Copy vertices. Vertices = new Vertices(vertices); } #pragma warning restore Normals = new Vertices(vertices.Count); // Compute normals. Ensure the edges have non-zero length. for (int i = 0; i < vertices.Count; ++i) { int i1 = i; int i2 = i + 1 < vertices.Count ? i + 1 : 0; Vector2 edge = Vertices[i2] - Vertices[i1]; Debug.Assert(edge.sqrMagnitude > Settings.Epsilon * Settings.Epsilon); Vector2 temp = new Vector2(edge.y, -edge.x); temp.Normalize(); Normals.Add(temp); } #if DEBUG // Ensure the polygon is convex and the interior // is to the left of each edge. for (int i = 0; i < Vertices.Count; ++i) { int i1 = i; int i2 = i + 1 < Vertices.Count ? i + 1 : 0; Vector2 edge = Vertices[i2] - Vertices[i1]; for (int j = 0; j < vertices.Count; ++j) { // Don't check vertices on the current edge. if (j == i1 || j == i2) { continue; } Vector2 r = Vertices[j] - Vertices[i1]; // Your polygon is non-convex (it has an indentation) or // has colinear edges. float s = edge.x * r.y - edge.y * r.x; Debug.Assert(s > 0.0f); } } #endif // Compute the polygon mass data ComputeProperties(); }
public static void Set(ref SimplexCache cache, DistanceProxy proxyA, ref Sweep sweepA, DistanceProxy proxyB, ref Sweep sweepB, float t1) { _localPoint = Vector2.zero; _proxyA = proxyA; _proxyB = proxyB; int count = cache.Count; Debug.Assert(0 < count && count < 3); _sweepA = sweepA; _sweepB = sweepB; Transform xfA, xfB; _sweepA.GetTransform(out xfA, t1); _sweepB.GetTransform(out xfB, t1); if (count == 1) { _type = SeparationFunctionType.Points; Vector2 localPointA = _proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); _axis = pointB - pointA; _axis.Normalize(); return; } else if (cache.IndexA[0] == cache.IndexA[1]) { // Two points on B and one on A. _type = SeparationFunctionType.FaceB; Vector2 localPointB1 = proxyB.Vertices[cache.IndexB[0]]; Vector2 localPointB2 = proxyB.Vertices[cache.IndexB[1]]; Vector2 a = localPointB2 - localPointB1; _axis = new Vector2(a.y, -a.x); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfB.R, _axis); _localPoint = 0.5f * (localPointB1 + localPointB2); Vector2 pointB = MathUtils.Multiply(ref xfB, _localPoint); Vector2 localPointA = proxyA.Vertices[cache.IndexA[0]]; Vector2 pointA = MathUtils.Multiply(ref xfA, localPointA); float s = Vector2.Dot(pointA - pointB, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } else { // Two points on A and one or two points on B. _type = SeparationFunctionType.FaceA; Vector2 localPointA1 = _proxyA.Vertices[cache.IndexA[0]]; Vector2 localPointA2 = _proxyA.Vertices[cache.IndexA[1]]; Vector2 a = localPointA2 - localPointA1; _axis = new Vector2(a.y, -a.x); _axis.Normalize(); Vector2 normal = MathUtils.Multiply(ref xfA.R, _axis); _localPoint = 0.5f * (localPointA1 + localPointA2); Vector2 pointA = MathUtils.Multiply(ref xfA, _localPoint); Vector2 localPointB = _proxyB.Vertices[cache.IndexB[0]]; Vector2 pointB = MathUtils.Multiply(ref xfB, localPointB); float s = Vector2.Dot(pointB - pointA, normal); if (s < 0.0f) { _axis = -_axis; s = -s; } return; } }
internal override bool SolvePositionConstraints() { // TODO_ERIN block solve with limit. COME ON ERIN Body b1 = BodyA; Body b2 = BodyB; float angularError = 0.0f; float positionError; // Solve angular limit constraint. if (_enableLimit && _limitState != LimitState.Inactive) { float angle = b2.Sweep.A - b1.Sweep.A - ReferenceAngle; float limitImpulse = 0.0f; if (_limitState == LimitState.Equal) { // Prevent large angular corrections float C = MathUtils.Clamp(angle - _lowerAngle, -Settings.MaxAngularCorrection, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; angularError = Math.Abs(C); } else if (_limitState == LimitState.AtLower) { float C = angle - _lowerAngle; angularError = -C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C + Settings.AngularSlop, -Settings.MaxAngularCorrection, 0.0f); limitImpulse = -_motorMass * C; } else if (_limitState == LimitState.AtUpper) { float C = angle - _upperAngle; angularError = C; // Prevent large angular corrections and allow some slop. C = MathUtils.Clamp(C - Settings.AngularSlop, 0.0f, Settings.MaxAngularCorrection); limitImpulse = -_motorMass * C; } b1.Sweep.A -= b1.InvI * limitImpulse; b2.Sweep.A += b2.InvI * limitImpulse; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } // Solve point-to-point constraint. { /*Transform xf1, xf2; * b1.GetTransform(out xf1); * b2.GetTransform(out xf2);*/ Vector2 r1 = MathUtils.Multiply(ref b1.Xf.R, LocalAnchorA - b1.LocalCenter); Vector2 r2 = MathUtils.Multiply(ref b2.Xf.R, LocalAnchorB - b2.LocalCenter); Vector2 C = b2.Sweep.C + r2 - b1.Sweep.C - r1; positionError = C.magnitude; float invMass1 = b1.InvMass, invMass2 = b2.InvMass; float invI1 = b1.InvI, invI2 = b2.InvI; // Handle large detachment. const float k_allowedStretch = 10.0f * Settings.LinearSlop; if (C.sqrMagnitude > k_allowedStretch * k_allowedStretch) { // Use a particle solution (no rotation). Vector2 u = C; u.Normalize(); float k = invMass1 + invMass2; Debug.Assert(k > Settings.Epsilon); float m = 1.0f / k; Vector2 impulse2 = m * (-C); const float k_beta = 0.5f; b1.Sweep.C -= k_beta * invMass1 * impulse2; b2.Sweep.C += k_beta * invMass2 * impulse2; C = b2.Sweep.C + r2 - b1.Sweep.C - r1; } Mat22 K1 = new Mat22(new Vector2(invMass1 + invMass2, 0.0f), new Vector2(0.0f, invMass1 + invMass2)); Mat22 K2 = new Mat22(new Vector2(invI1 * r1.y * r1.y, -invI1 * r1.x * r1.y), new Vector2(-invI1 * r1.x * r1.y, invI1 * r1.x * r1.x)); Mat22 K3 = new Mat22(new Vector2(invI2 * r2.y * r2.y, -invI2 * r2.x * r2.y), new Vector2(-invI2 * r2.x * r2.y, invI2 * r2.x * r2.x)); Mat22 Ka; Mat22.Add(ref K1, ref K2, out Ka); Mat22 K; Mat22.Add(ref Ka, ref K3, out K); Vector2 impulse = K.Solve(-C); b1.Sweep.C -= b1.InvMass * impulse; MathUtils.Cross(ref r1, ref impulse, out _tmpFloat1); b1.Sweep.A -= b1.InvI * /* r1 x impulse */ _tmpFloat1; b2.Sweep.C += b2.InvMass * impulse; MathUtils.Cross(ref r2, ref impulse, out _tmpFloat1); b2.Sweep.A += b2.InvI * /* r2 x impulse */ _tmpFloat1; b1.SynchronizeTransform(); b2.SynchronizeTransform(); } return(positionError <= Settings.LinearSlop && angularError <= Settings.AngularSlop); }
/// <summary> /// Ray-cast against the proxies in the tree. This relies on the callback /// to perform a exact ray-cast in the case were the proxy contains a Shape. /// The callback also performs the any collision filtering. This has performance /// roughly equal to k * log(n), where k is the number of collisions and n is the /// number of proxies in the tree. /// </summary> /// <param name="callback">A callback class that is called for each proxy that is hit by the ray.</param> /// <param name="input">The ray-cast input data. The ray extends from p1 to p1 + maxFraction * (p2 - p1).</param> public void RayCast(Func <RayCastInput, int, float> callback, ref RayCastInput input) { Vector2 p1 = input.Point1; Vector2 p2 = input.Point2; Vector2 r = p2 - p1; Debug.Assert(r.sqrMagnitude > 0.0f); r.Normalize(); // v is perpendicular to the segment. Vector2 absV = MathUtils.Abs(new Vector2(-r.y, r.x)); // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) float maxFraction = input.MaxFraction; // Build a bounding box for the segment. AABB segmentAABB = new AABB(); { Vector2 t = p1 + maxFraction * (p2 - p1); VectorMath.Min(ref p1, ref t, out segmentAABB.LowerBound); VectorMath.Max(ref p1, ref t, out segmentAABB.UpperBound); } _stack.Clear(); _stack.Push(_root); while (_stack.Count > 0) { int nodeId = _stack.Pop(); if (nodeId == NullNode) { continue; } DynamicTreeNode <T> node = _nodes[nodeId]; if (AABB.TestOverlap(ref node.AABB, ref segmentAABB) == false) { continue; } // Separating axis for segment (Gino, p80). // |dot(v, p1 - c)| > dot(|v|, h) Vector2 c = node.AABB.Center; Vector2 h = node.AABB.Extents; float separation = Math.Abs(Vector2.Dot(new Vector2(-r.y, r.x), p1 - c)) - Vector2.Dot(absV, h); if (separation > 0.0f) { continue; } if (node.IsLeaf()) { RayCastInput subInput; subInput.Point1 = input.Point1; subInput.Point2 = input.Point2; subInput.MaxFraction = maxFraction; float value = callback(subInput, nodeId); if (value == 0.0f) { // the client has terminated the raycast. return; } if (value > 0.0f) { // Update segment bounding box. maxFraction = value; Vector2 t = p1 + maxFraction * (p2 - p1); segmentAABB.LowerBound = Vector2.Min(p1, t); segmentAABB.UpperBound = Vector2.Max(p1, t); } } else { _stack.Push(node.Child1); _stack.Push(node.Child2); } } }
/// <summary> /// Triangulates a polygon using simple ear-clipping algorithm. Returns /// size of Triangle array unless the polygon can't be triangulated. /// This should only happen if the polygon self-intersects, /// though it will not _always_ return null for a bad polygon - it is the /// caller's responsibility to check for self-intersection, and if it /// doesn't, it should at least check that the return value is non-null /// before using. You're warned! /// /// Triangles may be degenerate, especially if you have identical points /// in the input to the algorithm. Check this before you use them. /// /// This is totally unoptimized, so for large polygons it should not be part /// of the simulation loop. /// /// Warning: Only works on simple polygons. /// </summary> /// <returns></returns> public static List <Triangle> TriangulatePolygon(Vertices vertices) { List <Triangle> results = new List <Triangle>(); if (vertices.Count < 3) { return(new List <Triangle>()); } //Recurse and split on pinch points Vertices pA, pB; Vertices pin = new Vertices(vertices); if (ResolvePinchPoint(pin, out pA, out pB)) { List <Triangle> mergeA = TriangulatePolygon(pA); List <Triangle> mergeB = TriangulatePolygon(pB); if (mergeA.Count == -1 || mergeB.Count == -1) { throw new Exception("Can't triangulate your polygon."); } for (int i = 0; i < mergeA.Count; ++i) { results.Add(new Triangle(mergeA[i])); } for (int i = 0; i < mergeB.Count; ++i) { results.Add(new Triangle(mergeB[i])); } return(results); } Triangle[] buffer = new Triangle[vertices.Count - 2]; int bufferSize = 0; float[] xrem = new float[vertices.Count]; float[] yrem = new float[vertices.Count]; for (int i = 0; i < vertices.Count; ++i) { xrem[i] = vertices[i].x; yrem[i] = vertices[i].y; } int vNum = vertices.Count; while (vNum > 3) { // Find an ear int earIndex = -1; float earMaxMinCross = -10.0f; for (int i = 0; i < vNum; ++i) { if (IsEar(i, xrem, yrem, vNum)) { int lower = Remainder(i - 1, vNum); int upper = Remainder(i + 1, vNum); Vector2 d1 = new Vector2(xrem[upper] - xrem[i], yrem[upper] - yrem[i]); Vector2 d2 = new Vector2(xrem[i] - xrem[lower], yrem[i] - yrem[lower]); Vector2 d3 = new Vector2(xrem[lower] - xrem[upper], yrem[lower] - yrem[upper]); d1.Normalize(); d2.Normalize(); d3.Normalize(); float cross12; MathUtils.Cross(ref d1, ref d2, out cross12); cross12 = Math.Abs(cross12); float cross23; MathUtils.Cross(ref d2, ref d3, out cross23); cross23 = Math.Abs(cross23); float cross31; MathUtils.Cross(ref d3, ref d1, out cross31); cross31 = Math.Abs(cross31); //Find the maximum minimum angle float minCross = Math.Min(cross12, Math.Min(cross23, cross31)); if (minCross > earMaxMinCross) { earIndex = i; earMaxMinCross = minCross; } } } // If we still haven't found an ear, we're screwed. // Note: sometimes this is happening because the // remaining points are collinear. Really these // should just be thrown out without halting triangulation. if (earIndex == -1) { for (int i = 0; i < bufferSize; i++) { results.Add(new Triangle(buffer[i])); } return(results); } // Clip off the ear: // - remove the ear tip from the list --vNum; float[] newx = new float[vNum]; float[] newy = new float[vNum]; int currDest = 0; for (int i = 0; i < vNum; ++i) { if (currDest == earIndex) { ++currDest; } newx[i] = xrem[currDest]; newy[i] = yrem[currDest]; ++currDest; } // - add the clipped triangle to the triangle list int under = (earIndex == 0) ? (vNum) : (earIndex - 1); int over = (earIndex == vNum) ? 0 : (earIndex + 1); Triangle toAdd = new Triangle(xrem[earIndex], yrem[earIndex], xrem[over], yrem[over], xrem[under], yrem[under]); buffer[bufferSize] = toAdd; ++bufferSize; // - replace the old list with the new one xrem = newx; yrem = newy; } Triangle tooAdd = new Triangle(xrem[1], yrem[1], xrem[2], yrem[2], xrem[0], yrem[0]); buffer[bufferSize] = tooAdd; ++bufferSize; for (int i = 0; i < bufferSize; i++) { results.Add(new Triangle(buffer[i])); } return(results); }
/// <summary> /// Collides and edge and a polygon, taking into account edge adjacency. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="xfA">The xf A.</param> /// <param name="polygonB">The polygon B.</param> /// <param name="xfB">The xf B.</param> public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB) { MathUtils.MultiplyT(ref xfA, ref xfB, out _xf); // Edge geometry _edgeA.V0 = edgeA.Vertex0; _edgeA.V1 = edgeA.Vertex1; _edgeA.V2 = edgeA.Vertex2; _edgeA.V3 = edgeA.Vertex3; Vector2 e = _edgeA.V2 - _edgeA.V1; // Normal points outwards in CCW order. _edgeA.Normal = new Vector2(e.y, -e.x); _edgeA.Normal.Normalize(); _edgeA.HasVertex0 = edgeA.HasVertex0; _edgeA.HasVertex3 = edgeA.HasVertex3; // Proxy for edge _proxyA.Vertices[0] = _edgeA.V1; _proxyA.Vertices[1] = _edgeA.V2; _proxyA.Normals[0] = _edgeA.Normal; _proxyA.Normals[1] = -_edgeA.Normal; _proxyA.Centroid = 0.5f * (_edgeA.V1 + _edgeA.V2); _proxyA.Count = 2; // Proxy for polygon _proxyB.Count = polygonB.Vertices.Count; _proxyB.Centroid = MathUtils.Multiply(ref _xf, ref polygonB.MassData.Centroid); for (int i = 0; i < polygonB.Vertices.Count; ++i) { _proxyB.Vertices[i] = MathUtils.Multiply(ref _xf, polygonB.Vertices[i]); _proxyB.Normals[i] = MathUtils.Multiply(ref _xf.R, polygonB.Normals[i]); } _radius = 2.0f * Settings.PolygonRadius; _limit11 = Vector2.zero; _limit12 = Vector2.zero; _limit21 = Vector2.zero; _limit22 = Vector2.zero; //Collide(ref manifold); inline start manifold.PointCount = 0; //ComputeAdjacency(); inline start Vector2 v0 = _edgeA.V0; Vector2 v1 = _edgeA.V1; Vector2 v2 = _edgeA.V2; Vector2 v3 = _edgeA.V3; // Determine allowable the normal regions based on adjacency. // Note: it may be possible that no normal is admissable. Vector2 centerB = _proxyB.Centroid; if (_edgeA.HasVertex0) { Vector2 e0 = v1 - v0; Vector2 e1 = v2 - v1; Vector2 n0 = new Vector2(e0.y, -e0.x); Vector2 n1 = new Vector2(e1.y, -e1.x); n0.Normalize(); n1.Normalize(); bool convex = MathUtils.Cross(n0, n1) >= 0.0f; bool front0 = Vector2.Dot(n0, centerB - v0) >= 0.0f; bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; if (convex) { if (front0 || front1) { _limit11 = n1; _limit12 = n0; } else { _limit11 = -n1; _limit12 = -n0; } } else { if (front0 && front1) { _limit11 = n0; _limit12 = n1; } else { _limit11 = -n0; _limit12 = -n1; } } } else { _limit11 = Vector2.zero; _limit12 = Vector2.zero; } if (_edgeA.HasVertex3) { Vector2 e1 = v2 - v1; Vector2 e2 = v3 - v2; Vector2 n1 = new Vector2(e1.y, -e1.x); Vector2 n2 = new Vector2(e2.y, -e2.x); n1.Normalize(); n2.Normalize(); bool convex = MathUtils.Cross(n1, n2) >= 0.0f; bool front1 = Vector2.Dot(n1, centerB - v1) >= 0.0f; bool front2 = Vector2.Dot(n2, centerB - v2) >= 0.0f; if (convex) { if (front1 || front2) { _limit21 = n2; _limit22 = n1; } else { _limit21 = -n2; _limit22 = -n1; } } else { if (front1 && front2) { _limit21 = n1; _limit22 = n2; } else { _limit21 = -n1; _limit22 = -n2; } } } else { _limit21 = Vector2.zero; _limit22 = Vector2.zero; } //ComputeAdjacency(); inline end //EPAxis edgeAxis = ComputeEdgeSeparation(); inline start EPAxis edgeAxis = ComputeEdgeSeparation(); // If no valid normal can be found than this edge should not collide. // This can happen on the middle edge of a 3-edge zig-zag chain. if (edgeAxis.Type == EPAxisType.Unknown) { return; } if (edgeAxis.Separation > _radius) { return; } EPAxis polygonAxis = ComputePolygonSeparation(); if (polygonAxis.Type != EPAxisType.Unknown && polygonAxis.Separation > _radius) { return; } // Use hysteresis for jitter reduction. const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; EPAxis primaryAxis; if (polygonAxis.Type == EPAxisType.Unknown) { primaryAxis = edgeAxis; } else if (polygonAxis.Separation > k_relativeTol * edgeAxis.Separation + k_absoluteTol) { primaryAxis = polygonAxis; } else { primaryAxis = edgeAxis; } EPProxy proxy1; EPProxy proxy2; FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>(); if (primaryAxis.Type == EPAxisType.EdgeA) { proxy1 = _proxyA; proxy2 = _proxyB; manifold.Type = ManifoldType.FaceA; } else { proxy1 = _proxyB; proxy2 = _proxyA; manifold.Type = ManifoldType.FaceB; } int edge1 = primaryAxis.Index; FindIncidentEdge(ref incidentEdge, proxy1, primaryAxis.Index, proxy2); int count1 = proxy1.Count; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; Vector2 v11 = proxy1.Vertices[iv1]; Vector2 v12 = proxy1.Vertices[iv2]; Vector2 tangent = v12 - v11; tangent.Normalize(); Vector2 normal = MathUtils.Cross(tangent, 1.0f); Vector2 planePoint = 0.5f * (v11 + v12); // Face offset. float frontOffset = Vector2.Dot(normal, v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -Vector2.Dot(tangent, v11) + _radius; float sideOffset2 = Vector2.Dot(tangent, v12) + _radius; // Clip incident edge against extruded edge1 side edges. FixedArray2<ClipVertex> clipPoints1; FixedArray2<ClipVertex> clipPoints2; int np; // Clip to box side 1 np = ClipSegmentToLine(out clipPoints1, ref incidentEdge, -tangent, sideOffset1, iv1); if (np < Settings.MaxManifoldPoints) { return; } // Clip to negative box side 1 np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2); if (np < Settings.MaxManifoldPoints) { return; } // Now clipPoints2 contains the clipped points. if (primaryAxis.Type == EPAxisType.EdgeA) { manifold.LocalNormal = normal; manifold.LocalPoint = planePoint; } else { manifold.LocalNormal = MathUtils.MultiplyT(ref _xf.R, ref normal); manifold.LocalPoint = MathUtils.MultiplyT(ref _xf, ref planePoint); } int pointCount = 0; for (int i1 = 0; i1 < Settings.MaxManifoldPoints; ++i1) { float separation = Vector2.Dot(normal, clipPoints2[i1].V) - frontOffset; if (separation <= _radius) { ManifoldPoint cp = manifold.Points[pointCount]; if (primaryAxis.Type == EPAxisType.EdgeA) { cp.LocalPoint = MathUtils.MultiplyT(ref _xf, clipPoints2[i1].V); cp.Id = clipPoints2[i1].ID; } else { cp.LocalPoint = clipPoints2[i1].V; cp.Id.Features.TypeA = clipPoints2[i1].ID.Features.TypeB; cp.Id.Features.TypeB = clipPoints2[i1].ID.Features.TypeA; cp.Id.Features.IndexA = clipPoints2[i1].ID.Features.IndexB; cp.Id.Features.IndexB = clipPoints2[i1].ID.Features.IndexA; } manifold.Points[pointCount] = cp; ++pointCount; } } manifold.PointCount = pointCount; //Collide(ref manifold); inline end }