internal float Evaluate(Transform TransformA, Transform TransformB) { switch (FaceType) { case Type.Points: { Vector2 axisA = TransformA.InverseTransformDirection(Axis); Vector2 axisB = TransformB.InverseTransformDirection(-Axis); Vector2 localPointA = ShapeA.GetSupportVertex(axisA); Vector2 localPointB = ShapeB.GetSupportVertex(axisB); Vector2 pointA = TransformA.TransformPoint(localPointA); Vector2 pointB = TransformB.TransformPoint(localPointB); float separation = Vector2.Dot(pointB - pointA, Axis); return(separation); } case Type.FaceA: { Vector2 normal = TransformA.TransformDirection(Axis); Vector2 pointA = TransformA.TransformPoint(LocalPoint); Vector2 axisB = TransformB.InverseTransformDirection(-normal); Vector2 localPointB = ShapeB.GetSupportVertex(axisB); Vector2 pointB = TransformB.TransformPoint(localPointB); float separation = Vector2.Dot(pointB - pointA, normal); return(separation); } case Type.FaceB: { Vector2 normal = TransformB.TransformDirection(Axis); Vector2 pointB = TransformB.TransformPoint(LocalPoint); Vector2 axisA = TransformA.InverseTransformDirection(-normal); Vector2 localPointA = ShapeA.GetSupportVertex(axisA); Vector2 pointA = TransformA.TransformPoint(localPointA); float separation = Vector2.Dot(pointA - pointB, normal); return(separation); } default: Box2DNetDebug.Assert(false); return(0.0f); } }
public override bool TestPoint(Transform xf, Vector2 p) { Vector2 pLocal = xf.InverseTransformDirection(p - xf.position); int vc = _vertexCount; for (int i = 0; i < vc; ++i) { float dot = Vector2.Dot(_normals[i], pLocal - _vertices[i]); if (dot > 0.0f) { return(false); } } return(true); }
public static void FindIncidentEdge(out ClipVertex[] c, PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { int count1 = poly1._vertexCount; Vector2[] normals1 = poly1._normals; int count2 = poly2._vertexCount; Vector2[] vertices2 = poly2._vertices; Vector2[] normals2 = poly2._normals; Box2DNetDebug.Assert(0 <= edge1 && edge1 < count1); // Get the normal of the reference edge in poly2's frame. Vector2 normal1 = xf2.InverseTransformDirection(xf1.TransformDirection(normals1[edge1])); // Find the incident edge on poly2. int index = 0; float minDot = Settings.FLT_MAX; for (int i = 0; i < count2; ++i) { float dot = Vector2.Dot(normal1, normals2[i]); if (dot < minDot) { minDot = dot; index = i; } } // Build the clip vertices for the incident edge. int i1 = index; int i2 = i1 + 1 < count2 ? i1 + 1 : 0; c = new ClipVertex[2]; c[0].V = Common.Math.Mul(xf2, vertices2[i1]); c[0].ID.Features.ReferenceEdge = (byte)edge1; c[0].ID.Features.IncidentEdge = (byte)i1; c[0].ID.Features.IncidentVertex = 0; c[1].V = Common.Math.Mul(xf2, vertices2[i2]); c[1].ID.Features.ReferenceEdge = (byte)edge1; c[1].ID.Features.IncidentEdge = (byte)i2; c[1].ID.Features.IncidentVertex = 1; }
/// <summary> /// Find the separation between poly1 and poly2 for a give edge normal on poly1. /// </summary> public static float EdgeSeparation(PolygonShape poly1, Transform xf1, int edge1, PolygonShape poly2, Transform xf2) { int count1 = poly1._vertexCount; Vector2[] vertices1 = poly1._vertices; Vector2[] normals1 = poly1._normals; int count2 = poly2._vertexCount; Vector2[] vertices2 = poly2._vertices; //This fixed it, not sure why it was broken if (normals1.Length - 1 <= edge1 || edge1 < 0) { return(0); } // Convert normal from poly1's frame into poly2's frame. Vector2 normal1World = xf1.TransformDirection(normals1[edge1]); Vector2 normal1 = xf2.InverseTransformDirection(normal1World); // Find support vertex on poly2 for -normal. int index = 0; float minDot = Common.Settings.FLT_MAX; for (int i = 0; i < count2; ++i) { float dot = Vector2.Dot(vertices2[i], normal1); if (dot < minDot) { minDot = dot; index = i; } } Vector2 v1 = xf1.TransformPoint(vertices1[edge1]); Vector2 v2 = xf2.TransformPoint(vertices2[index]); float separation = Vector2.Dot(v2 - v1, normal1World); return(separation); }
static void Distance(out DistanceOutput output, ref SimplexCache cache, ref DistanceInput input, Shape shapeA, Shape shapeB) { output = new DistanceOutput(); Transform transformA = input.TransformA; Transform transformB = input.TransformB; // Initialize the simplex. Simplex simplex = new Simplex(); #if ALLOWUNSAFE fixed(SimplexCache *sPtr = &cache) { simplex.ReadCache(sPtr, shapeA, transformA, shapeB, transformB); } #else simplex.ReadCache(cache, shapeA, transformA, shapeB, transformB); #endif // Get simplex vertices as an array. #if ALLOWUNSAFE SimplexVertex *vertices = &simplex._v1; #else SimplexVertex[] vertices = new SimplexVertex[] { simplex._v1, simplex._v2, simplex._v3 }; #endif // These store the vertices of the last simplex so that we // can check for duplicates and prevent cycling. #if ALLOWUNSAFE int *lastA = stackalloc int[4], lastB = stackalloc int[4]; #else int[] lastA = new int[4]; int[] lastB = new int[4]; #endif // ALLOWUNSAFE int lastCount; // Main iteration loop. int iter = 0; const int k_maxIterationCount = 20; while (iter < k_maxIterationCount) { // Copy simplex so we can identify duplicates. lastCount = simplex._count; int i; for (i = 0; i < lastCount; ++i) { lastA[i] = vertices[i].indexA; lastB[i] = vertices[i].indexB; } switch (simplex._count) { case 1: break; case 2: simplex.Solve2(); break; case 3: simplex.Solve3(); break; default: 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(); float distanceSqr = p.LengthSquared(); // Ensure the search direction is numerically fit. if (distanceSqr < Common.Settings.FLT_EPSILON_SQUARED) { // 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. #if ALLOWUNSAFE SimplexVertex *vertex = vertices + simplex._count; vertex->indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); vertex->wA = transformA.TransformPoint(shapeA.GetVertex(vertex->indexA)); //Vec2 wBLocal; vertex->indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); vertex->wB = transformB.TransformPoint(shapeB.GetVertex(vertex->indexB)); vertex->w = vertex->wB - vertex->wA; #else SimplexVertex vertex = vertices[simplex._count - 1]; vertex.indexA = shapeA.GetSupport(transformA.InverseTransformDirection(p)); vertex.wA = transformA.TransformPoint(shapeA.GetVertex(vertex.indexA)); //Vec2 wBLocal; vertex.indexB = shapeB.GetSupport(transformB.InverseTransformDirection(-p)); vertex.wB = transformB.TransformPoint(shapeB.GetVertex(vertex.indexB)); vertex.w = vertex.wB - vertex.wA; #endif // ALLOWUNSAFE // Iteration count is equated to the number of support point calls. ++iter; // Check for convergence. #if ALLOWUNSAFE float lowerBound = Vector2.Dot(p, vertex->w); #else float lowerBound = Vector2.Dot(p, vertex.w); #endif float upperBound = distanceSqr; const float k_relativeTolSqr = 0.01f * 0.01f; // 1:100 if (upperBound - lowerBound <= k_relativeTolSqr * upperBound) { // Converged! break; } // Check for duplicate support points. bool duplicate = false; for (i = 0; i < lastCount; ++i) { #if ALLOWUNSAFE if (vertex->indexA == lastA[i] && vertex->indexB == lastB[i]) #else if (vertex.indexA == lastA[i] && vertex.indexB == lastB[i]) #endif { 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; } #if ALLOWUNSAFE fixed(DistanceOutput *doPtr = &output) { // Prepare output. simplex.GetWitnessPoints(&doPtr->PointA, &doPtr->PointB); doPtr->Distance = Vector2.Distance(doPtr->PointA, doPtr->PointB); doPtr->Iterations = iter; } fixed(SimplexCache *sPtr = &cache) { // Cache the simplex. simplex.WriteCache(sPtr); } #else // Prepare output. simplex.GetWitnessPoints(ref output.PointA, ref output.PointB); output.Distance = Box2DNet.Common.Math.Distance(output.PointA, output.PointB); output.Iterations = iter; // Cache the simplex. simplex.WriteCache(cache); #endif // Apply radii if requested. if (input.UseRadii) { float rA = shapeA._radius; float rB = shapeB._radius; if (output.Distance > rA + rB && output.Distance > Common.Settings.FLT_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; } } }
public override float ComputeSubmergedArea(Vector2 normal, float offset, Transform xf, out Vector2 c) { //Transform plane into shape co-ordinates Vector2 normalL = xf.InverseTransformDirection(normal); float offsetL = offset - Vector2.Dot(normal, xf.position); float[] depths = new float[Common.Settings.MaxPolygonVertices]; int diveCount = 0; int intoIndex = -1; int outoIndex = -1; bool lastSubmerged = false; int i; for (i = 0; i < _vertexCount; i++) { depths[i] = Vector2.Dot(normalL, _vertices[i]) - offsetL; bool isSubmerged = depths[i] < -Common.Settings.FLT_EPSILON; if (i > 0) { if (isSubmerged) { if (!lastSubmerged) { intoIndex = i - 1; diveCount++; } } else { if (lastSubmerged) { outoIndex = i - 1; diveCount++; } } } lastSubmerged = isSubmerged; } switch (diveCount) { case 0: if (lastSubmerged) { //Completely submerged MassData md; ComputeMass(out md, 1f); c = xf.TransformPoint(md.Center); return(md.Mass); } else { //Completely dry c = new Vector2(); return(0); } break; case 1: if (intoIndex == -1) { intoIndex = _vertexCount - 1; } else { outoIndex = _vertexCount - 1; } break; } int intoIndex2 = (intoIndex + 1) % _vertexCount; int outoIndex2 = (outoIndex + 1) % _vertexCount; float intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]); float outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]); Vector2 intoVec = new Vector2(_vertices[intoIndex].X * (1 - intoLambda) + _vertices[intoIndex2].X * intoLambda, _vertices[intoIndex].Y * (1 - intoLambda) + _vertices[intoIndex2].Y * intoLambda); Vector2 outoVec = new Vector2(_vertices[outoIndex].X * (1 - outoLambda) + _vertices[outoIndex2].X * outoLambda, _vertices[outoIndex].Y * (1 - outoLambda) + _vertices[outoIndex2].Y * outoLambda); //Initialize accumulator float area = 0; Vector2 center = Vector2.Zero; Vector2 p2 = _vertices[intoIndex2]; Vector2 p3; const float k_inv3 = 1.0f / 3.0f; //An awkward loop from intoIndex2+1 to outIndex2 i = intoIndex2; while (i != outoIndex2) { i = (i + 1) % _vertexCount; if (i == outoIndex2) { p3 = outoVec; } else { p3 = _vertices[i]; } //Add the triangle formed by intoVec,p2,p3 { Vector2 e1 = p2 - intoVec; Vector2 e2 = p3 - intoVec; float D = e1.Cross(e2); float triangleArea = 0.5f * D; area += triangleArea; // Area weighted centroid center += triangleArea * k_inv3 * (intoVec + p2 + p3); } // p2 = p3; } //Normalize and Transform centroid center *= 1.0f / area; c = xf.TransformPoint(center); return(area); }
public override SegmentCollide TestSegment(Transform xf, out float lambda, out Vector2 normal, Segment segment, float maxLambda) { lambda = 0f; normal = Vector2.Zero; float lower = 0.0f, upper = maxLambda; Vector2 p1 = xf.InverseTransformDirection(segment.P1 - xf.position); Vector2 p2 = xf.InverseTransformDirection(segment.P2 - xf.position); Vector2 d = p2 - p1; int index = -1; for (int i = 0; i < _vertexCount; ++i) { // p = p1 + a * d // dot(normal, p - v) = 0 // dot(normal, p1 - v) + a * dot(normal, d) = 0 float numerator = Vector2.Dot(_normals[i], _vertices[i] - p1); float denominator = Vector2.Dot(_normals[i], d); if (denominator == 0.0f) { if (numerator < 0.0f) { return(SegmentCollide.MissCollide); } } else { // Note: we want this predicate without division: // lower < numerator / denominator, where denominator < 0 // Since denominator < 0, we have to flip the inequality: // lower < numerator / denominator <==> denominator * lower > numerator. if (denominator < 0.0f && numerator < lower * denominator) { // Increase lower. // The segment enters this half-space. lower = numerator / denominator; index = i; } else if (denominator > 0.0f && numerator < upper * denominator) { // Decrease upper. // The segment exits this half-space. upper = numerator / denominator; } } if (upper < lower) { return(SegmentCollide.MissCollide); } } if (index >= 0) { lambda = lower; normal = xf.TransformDirection(_normals[index]); return(SegmentCollide.HitCollide); } lambda = 0f; return(SegmentCollide.StartInsideCollide); }
/// <summary> /// Find the max separation between poly1 and poly2 using edge normals from poly1. /// </summary> public static float FindMaxSeparation(ref int edgeIndex, PolygonShape poly1, Transform xf1, PolygonShape poly2, Transform xf2) { int count1 = poly1._vertexCount; Vector2[] normals1 = poly1._normals; // Vector pointing from the centroid of poly1 to the centroid of poly2. Vector2 d = xf2.TransformPoint(poly2._centroid) - xf1.TransformPoint(poly2._centroid); Vector2 dLocal1 = xf1.InverseTransformDirection(d); // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; float maxDot = -Common.Settings.FLT_MAX; for (int i = 0; i < count1; ++i) { float dot = Vector2.Dot(normals1[i], dLocal1); if (dot > maxDot) { maxDot = dot; edge = i; } } // Get the separation for the edge normal. float s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float sPrev = Collision.EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; float sNext = Collision.EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2); // Find the best edge and the search direction. int bestEdge; float bestSeparation; int increment; if (sPrev > s && sPrev > sNext) { increment = -1; bestEdge = prevEdge; bestSeparation = sPrev; } else if (sNext > s) { increment = 1; bestEdge = nextEdge; bestSeparation = sNext; } else { edgeIndex = edge; return(s); } // Perform a local search for the best edge normal. for (; ;) { if (increment == -1) { edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1; } else { edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0; } s = Collision.EdgeSeparation(poly1, xf1, edge, poly2, xf2); if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } edgeIndex = bestEdge; return(bestSeparation); }
/// <summary> /// Gets a local vector given a world vector. /// </summary> /// <param name="worldVector">A vector in world coordinates.</param> /// <returns>Return the corresponding local vector.</returns> public Vector2 GetLocalVector(Vector2 worldVector) { return(_xf.InverseTransformDirection(worldVector)); }