public static b2Vec2 b2Mul(b2Transform T, b2Vec2 v) { float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; return(new b2Vec2(x, y)); }
public static b2Transform b2MulT(b2Transform A, b2Transform B) { b2Transform C = b2Transform.Create(); C.q = b2MulT(A.q, B.q); C.p = b2MulT(A.q, B.p - A.p); return C; }
public static b2Transform Create() { var transform = new b2Transform(); transform.p = b2Vec2.Zero; transform.q = b2Rot.Create(); return transform; }
// v2 = A.q.Rot(B.q.Rot(v1) + B.p) + A.p // = (A.q * B.q).Rot(v1) + A.q.Rot(B.p) + A.p public static b2Transform b2Mul(b2Transform A, b2Transform B) { b2Transform C = b2Transform.Create(); C.q = b2Mul(A.q, B.q); C.p = b2Mul(A.q, B.p) + A.p; return(C); }
public override b2AABB ComputeAABB(b2Transform transform, int childIndex) { b2Vec2 p = transform.p + b2Math.b2Mul(transform.q, m_p); b2AABB aabb = new b2AABB(); aabb.lowerBound.Set(p.x - m_radius, p.y - m_radius); aabb.upperBound.Set(p.x + m_radius, p.y + m_radius); return (aabb); }
public static b2Vec2 b2Mul(ref b2Transform T, ref b2Vec2 v) { b2Vec2 b; b.x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; b.y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; return(b); }
public static b2Transform b2MulT(b2Transform A, b2Transform B) { b2Transform C; C.q = b2MulT(A.q, B.q); C.p = b2MulT(A.q, B.p - A.p); return(C); }
public static b2Transform Create() { var transform = new b2Transform(); transform.p = b2Vec2.Zero; transform.q = b2Rot.Create(); return(transform); }
public static b2Vec2 b2Mul(b2Transform T, b2Vec2 v) { float x = (T.q.c * v.x - T.q.s * v.y) + T.p.x; float y = (T.q.s * v.x + T.q.c * v.y) + T.p.y; b2Vec2 b = b2Vec2.Zero; b.Set(x, y); return b; }
public override void Evaluate(ref b2Manifold manifold, ref b2Transform xfA, ref b2Transform xfB) { b2ChainShape chain = (b2ChainShape)m_fixtureA.Shape; b2EdgeShape edge; edge = chain.GetChildEdge(m_indexA); b2Collision.b2CollideEdgeAndCircle(ref manifold, edge, ref xfA, (b2CircleShape)m_fixtureB.Shape, ref xfB); }
/*public b2Sweep() { localCenter = new b2Vec2(); c0 = new b2Vec2(); c = new b2Vec2(); a0 = 0f; a = 0f; alpha0 = 0f; }*/ /// Get the interpolated transform at a specific time. /// @param beta is a factor in [0,1], where 0 indicates alpha0. public void GetTransform(b2Transform xfb, float beta) { xfb.p = (1.0f - beta) * c0 + beta * c; float angle = (1.0f - beta) * a0 + beta * a; xfb.q.Set(angle); // Shift to origin xfb.p -= b2Math.b2Mul(xfb.q, localCenter); }
public static b2Vec2 b2MulT(b2Transform T, b2Vec2 v) { float px = v.x - T.p.x; float py = v.y - T.p.y; float x = (T.q.c * px + T.q.s * py); float y = (-T.q.s * px + T.q.c * py); return(new b2Vec2(x, y)); }
public static b2Vec2 b2MulT(b2Transform T, b2Vec2 v) { float px = v.x - T.p.x; float py = v.y - T.p.y; float x = (T.q.c * px + T.q.s * py); float y = (-T.q.s * px + T.q.c * py); b2Vec2 b = b2Vec2.Zero; b.Set(x, y); return b; }
public static b2Vec2 b2MulT(ref b2Transform T, ref b2Vec2 v) { b2Vec2 b; float px = v.x - T.p.x; float py = v.y - T.p.y; b.x = (T.q.c * px + T.q.s * py); b.y = (-T.q.s * px + T.q.c * py); return(b); }
/*public b2Sweep() * { * localCenter = new b2Vec2(); * c0 = new b2Vec2(); * c = new b2Vec2(); * a0 = 0f; * a = 0f; * alpha0 = 0f; * }*/ /// Get the interpolated transform at a specific time. /// @param beta is a factor in [0,1], where 0 indicates alpha0. public void GetTransform(b2Transform xfb, float beta) { xfb.p = (1.0f - beta) * c0 + beta * c; float angle = (1.0f - beta) * a0 + beta * a; xfb.q.Set(angle); // Shift to origin xfb.p -= b2Math.b2Mul(xfb.q, localCenter); }
public override bool TestPoint(ref b2Transform transform, b2Vec2 p) { b2Vec2 tx = b2Math.b2Mul(ref transform.q, ref Position); b2Vec2 center; center.x = transform.p.x + tx.x; center.y = transform.p.y + tx.y; // b2Vec2 center = transform.p + tx; b2Vec2 d; // = p - center; d.x = p.x - center.x; d.y = p.y - center.y; return d.LengthSquared <= Radius * Radius; }
public override bool TestPoint(b2Transform transform, b2Vec2 p) { b2Vec2 tx = b2Math.b2Mul(transform.q, m_p); b2Vec2 center; center.x = transform.p.x + tx.x; center.y = transform.p.y + tx.y; // b2Vec2 center = transform.p + tx; b2Vec2 d; // = p - center; d.x = p.x - center.x; d.y = p.y - center.y; return d.LengthSquared <= m_radius * m_radius; }
public override void DrawTransform(b2Transform xf) { float axisScale = 0.4f; b2Vec2 p1 = xf.p; b2Vec2 col1 = new b2Vec2(xf.q.c, xf.q.s); b2Vec2 col2 = new b2Vec2(-xf.q.s, xf.q.c); b2Vec2 p2 = p1 + axisScale * col1; DrawSegment(p1, p2, new b2Color(1f, 0f, 0f)); p2 = p1 + axisScale * col2; DrawSegment(p1, p2, new b2Color(0f, 0f, 1f)); }
// Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input, b2Transform transform, int childIndex) { output = b2RayCastOutput.Zero; b2Vec2 tx = b2Math.b2Mul(transform.q, m_p); // b2Vec2 position = transform.p + tx; b2Vec2 position; position.x = transform.p.x + tx.x; position.y = transform.p.y + tx.y; // b2Vec2 s = input.p1 - position; b2Vec2 s; s.x = input.p1.x - position.x; s.y = input.p1.y - position.y; float b = s.LengthSquared - m_radius * m_radius; // Solve quadratic equation. b2Vec2 r; r.x = input.p2.x - input.p1.x; r.y = input.p2.y - input.p1.y; // b2Vec2 r = input.p2 - input.p1; float c = s.x * r.x + s.y * r.y; // b2Math.b2Dot(ref s, ref r); float rr = r.LengthSquared; // b2Math.b2Dot(r, r); float sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < b2Settings.b2_epsilon) { return false; } // Find the point of intersection of the line with the circle. float a = -(c + b2Math.b2Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.maxFraction * rr) { a /= rr; output.fraction = a; output.normal.x = s.x + a * r.x; output.normal.y = s.y + a * r.y; // output.normal = s + a * r; output.normal.Normalize(); return true; } return false; }
/// Get the interpolated transform at a specific time. /// @param beta is a factor in [0,1], where 0 indicates alpha0. public void GetTransform(out b2Transform xfb, float beta) { float x = (1.0f - beta) * c0.x + beta * c.x; float y = (1.0f - beta) * c0.y + beta * c.y; float angle = (1.0f - beta) * a0 + beta * a; float sin = (float)Math.Sin(angle); float cos = (float)Math.Cos(angle); // Shift to origin xfb.p.x = x - (cos * localCenter.x - sin * localCenter.y); xfb.p.y = y - (sin * localCenter.x + cos * localCenter.y); xfb.q.s = sin; xfb.q.c = cos; }
// Collision Detection in Interactive 3D Environments by Gino van den Bergen // From Section 3.1.2 // x = s + a * r // norm(x) = radius public virtual bool RayCast(out b2RayCastOutput output, b2RayCastInput input, b2Transform transform, int childIndex) { output = b2RayCastOutput.Zero; b2Vec2 position = transform.p + b2Math.b2Mul(transform.q, m_p); b2Vec2 s = input.p1 - position; float b = b2Math.b2Dot(s, s) - m_radius * m_radius; // Solve quadratic equation. b2Vec2 r = input.p2 - input.p1; float c = b2Math.b2Dot(s, r); float rr = b2Math.b2Dot(r, r); float sigma = c * c - rr * b; // Check for negative discriminant and short segment. if (sigma < 0.0f || rr < b2Settings.b2_epsilon) { return false; } // Find the point of intersection of the line with the circle. float a = -(c + b2Math.b2Sqrt(sigma)); // Is the intersection point on the segment? if (0.0f <= a && a <= input.maxFraction * rr) { a /= rr; output.fraction = a; output.normal = s + a * r; output.normal.Normalize(); return true; } return false; }
/// Determine if two generic shapes overlap. public static bool b2TestOverlap(b2Shape shapeA, int indexA, b2Shape shapeB, int indexB, ref b2Transform xfA, ref b2Transform xfB) { b2DistanceInput input = b2DistanceInput.Create(); input.proxyA = b2DistanceProxy.Create(shapeA, indexA); input.proxyB = b2DistanceProxy.Create(shapeB, indexB); input.transformA = xfA; input.transformB = xfB; input.useRadii = true; b2SimplexCache cache = b2SimplexCache.Create(); b2DistanceOutput output = new b2DistanceOutput(); b2Simplex.b2Distance(ref output, ref cache, ref input); // Console.WriteLine("{2} vs {3}: distance={0} after {1} iters", output.distance, output.iterations, shapeA.ShapeType, shapeB.ShapeType); return output.distance < 10.0f * b2Settings.b2_epsilon; }
/// Compute the collision manifold between an edge and a circle. public static void b2CollideEdgeAndPolygon(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA, b2PolygonShape polygonB, ref b2Transform xfB) { b2EPCollider b = new b2EPCollider(); b.Collide(ref manifold, edgeA, ref xfA, polygonB, ref xfB); }
/// Compute the collision manifold between an edge and a circle. public static void b2CollideEdgeAndCircle(ref b2Manifold manifold, b2EdgeShape edgeA, ref b2Transform xfA, b2CircleShape circleB, ref b2Transform xfB) { manifold.pointCount = 0; // Compute circle in frame of edge b2Vec2 Q = b2Math.b2MulT(xfA, b2Math.b2Mul(xfB, circleB.Position)); b2Vec2 A = edgeA.Vertex1, B = edgeA.Vertex2; b2Vec2 e = B - A; b2Vec2 diff; // Barycentric coordinates diff = B - Q; float u = b2Math.b2Dot(ref e, ref diff); // B - Q); diff = Q - A; float v = b2Math.b2Dot(ref e, ref diff); // Q - A); float radius = edgeA.Radius + circleB.Radius; b2ContactFeature cf = b2ContactFeature.Zero; cf.indexB = 0; cf.typeB = b2ContactFeatureType.e_vertex; // Region A if (v <= 0.0f) { b2Vec2 P = A; b2Vec2 d = Q - P; float dd = d.LengthSquared; // b2Math.b2Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA.HasVertex0) { b2Vec2 A1 = edgeA.Vertex0; b2Vec2 B1 = A; b2Vec2 e1 = B1 - A1; diff = B1 - Q; float u1 = b2Math.b2Dot(ref e1, ref diff); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.indexA = 0; cf.typeA = b2ContactFeatureType.e_vertex; manifold.pointCount = 1; manifold.type = b2ManifoldType.e_circles; manifold.localNormal.SetZero(); manifold.localPoint = P; manifold.points[0].id.key = 0; manifold.points[0].id.Set(cf); manifold.points[0].localPoint = circleB.Position; return; } // Region B if (u <= 0.0f) { b2Vec2 P = B; b2Vec2 d = Q - P; float dd = d.LengthSquared; // b2Math.b2Dot(d, d); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA.HasVertex3) { b2Vec2 B2 = edgeA.Vertex3; b2Vec2 A2 = B; b2Vec2 e2 = B2 - A2; diff = Q - A2; float v2 = b2Math.b2Dot(ref e2, ref diff); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.indexA = 1; cf.typeA = b2ContactFeatureType.e_vertex; manifold.pointCount = 1; manifold.type = b2ManifoldType.e_circles; manifold.localNormal.SetZero(); manifold.localPoint = P; manifold.points[0].id.key = 0; manifold.points[0].id.Set(cf); manifold.points[0].localPoint = circleB.Position; return; } // Region AB float den = e.Length; // b2Math.b2Dot(e, e); System.Diagnostics.Debug.Assert(den > 0.0f); b2Vec2 xP = (1.0f / den) * (u * A + v * B); b2Vec2 xd = Q - xP; float xdd = xd.LengthSquared; // b2Math.b2Dot(xd, xd); if (xdd > radius * radius) { return; } b2Vec2 n = b2Vec2.Zero; // new b2Vec2(-e.y, e.x); n.m_x = -e.y; n.m_y = e.x; diff = Q - A; if (b2Math.b2Dot(ref n, ref diff) < 0.0f) { // n.Set(-n.x, -n.y); n.Set(-n.m_x, -n.m_y); } n.Normalize(); cf.indexA = 0; cf.typeA = b2ContactFeatureType.e_face; manifold.pointCount = 1; manifold.type = b2ManifoldType.e_faceA; manifold.localNormal = n; manifold.localPoint = A; manifold.points[0].id.key = 0; manifold.points[0].id.Set(cf); manifold.points[0].localPoint = circleB.Position; }
public static float b2EdgeSeparation(b2PolygonShape poly1, b2Transform xf1, int edge1, b2PolygonShape poly2, b2Transform xf2) { b2Vec2[] vertices1 = poly1.Vertices; b2Vec2[] normals1 = poly1.Normals; int count2 = poly2.VertexCount; b2Vec2[] vertices2 = poly2.Vertices; // Convert normal from poly1's frame into poly2's frame. b2Vec2 normal1World = b2Math.b2Mul(xf1.q, normals1[edge1]); b2Vec2 normal1 = b2Math.b2MulT(xf2.q, normal1World); // Find support vertex on poly2 for -normal. int index = 0; float minDot = b2Settings.b2_maxFloat; for (int i = 0; i < count2; ++i) { float dot = b2Math.b2Dot(ref vertices2[i], ref normal1); if (dot < minDot) { minDot = dot; index = i; } } b2Vec2 v1 = b2Math.b2Mul(xf1, vertices1[edge1]); b2Vec2 v2 = b2Math.b2Mul(xf2, vertices2[index]); float separation = b2Math.b2Dot(v2 - v1, normal1World); return separation; }
/// Test a point for containment in this shape. This only works for convex shapes. /// @param xf the shape world transform. /// @param p a point in world coordinates. public abstract bool TestPoint(b2Transform xf, b2Vec2 p);
public override bool TestPoint(b2Transform xf, b2Vec2 p) { return false; }
public override b2AABB ComputeAABB(b2Transform xf, int childIndex) { int i1 = childIndex; int i2 = childIndex + 1; if (i2 == m_count) { i2 = 0; } b2Vec2 v1 = b2Math.b2Mul(xf, m_vertices[i1]); b2Vec2 v2 = b2Math.b2Mul(xf, m_vertices[i2]); b2AABB aabb = b2AABB.Default; aabb.Set(b2Math.b2Min(v1, v2),b2Math.b2Max(v1, v2)); return (aabb); }
// Find the max separation between poly1 and poly2 using edge normals from poly1. public static float b2FindMaxSeparation(out int edgeIndex, b2PolygonShape poly1, ref b2Transform xf1, b2PolygonShape poly2, ref b2Transform xf2) { int count1 = poly1.VertexCount; b2Vec2[] normals1 = poly1.Normals; // Vector pointing from the centroid of poly1 to the centroid of poly2. b2Vec2 d = b2Math.b2Mul(xf2, poly2.Centroid) - b2Math.b2Mul(xf1, poly1.Centroid); b2Vec2 dLocal1 = b2Math.b2MulT(xf1.q, d); // Find edge normal on poly1 that has the largest projection onto d. int edge = 0; float maxDot = -b2Settings.b2_maxFloat; for (int i = 0; i < count1; ++i) { float dot = b2Math.b2Dot(normals1[i], dLocal1); if (dot > maxDot) { maxDot = dot; edge = i; } } // Get the separation for the edge normal. float s = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2); // Check the separation for the previous edge normal. int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1; float sPrev = b2EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2); // Check the separation for the next edge normal. int nextEdge = edge + 1 < count1 ? edge + 1 : 0; float sNext = b2EdgeSeparation(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 = b2EdgeSeparation(poly1, xf1, edge, poly2, xf2); if (s > bestSeparation) { bestEdge = edge; bestSeparation = s; } else { break; } } edgeIndex = bestEdge; return bestSeparation; }
/// Draw a transform. Choose your own length scale. /// @param xf a transform. public abstract void DrawTransform(b2Transform xf);
public static void b2FindIncidentEdge(b2ClipVertex[] c, b2PolygonShape poly1, b2Transform xf1, int edge1, b2PolygonShape poly2, b2Transform xf2) { b2Vec2[] normals1 = poly1.Normals; int count2 = poly2.VertexCount; b2Vec2[] vertices2 = poly2.Vertices; b2Vec2[] normals2 = poly2.Normals; // Get the normal of the reference edge in poly2's frame. b2Vec2 normal1 = b2Math.b2MulT(xf2.q, b2Math.b2Mul(xf1.q, normals1[edge1])); // Find the incident edge on poly2. int index = 0; float minDot = b2Settings.b2_maxFloat; for (int i = 0; i < count2; ++i) { float dot = b2Math.b2Dot(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[0].v = b2Math.b2Mul(xf2, vertices2[i1]); c[0].id.indexA = (byte)edge1; c[0].id.indexB = (byte)i1; c[0].id.typeA = b2ContactFeatureType.e_face; c[0].id.typeB = b2ContactFeatureType.e_vertex; c[1].v = b2Math.b2Mul(xf2, vertices2[i2]); c[1].id.indexA = (byte)edge1; c[1].id.indexB = (byte)i2; c[1].id.typeA = b2ContactFeatureType.e_face; c[1].id.typeB = b2ContactFeatureType.e_vertex; }
public override b2AABB ComputeAABB(b2Transform xf, int childIndex) { b2Vec2 v1 = b2Math.b2Mul(xf, m_vertex1); b2Vec2 v2 = b2Math.b2Mul(xf, m_vertex2); b2Vec2 lower = b2Math.b2Min(v1, v2); b2Vec2 upper = b2Math.b2Max(v1, v2); b2Vec2 r = new b2Vec2(m_radius, m_radius); b2AABB aabb = new b2AABB(); aabb.lowerBound = lower - r; aabb.upperBound = upper + r; return(aabb); }
public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input, b2Transform xf, int childIndex) { b2EdgeShape edgeShape = new b2EdgeShape(); output = b2RayCastOutput.Zero; int i1 = childIndex; int i2 = childIndex + 1; if (i2 == m_count) { i2 = 0; } edgeShape.Vertex1 = m_vertices[i1]; edgeShape.Vertex2 = m_vertices[i2]; b2RayCastOutput co = b2RayCastOutput.Zero; bool b = edgeShape.RayCast(out co, input, xf, 0); output = co; return (b); }
// p = p1 + t * d // v = v1 + s * e // p1 + t * d = v1 + s * e // s * e - t * d = p1 - v1 public override bool RayCast(out b2RayCastOutput output, b2RayCastInput input, b2Transform xf, int childIndex) { output = b2RayCastOutput.Zero; // Put the ray into the edge's frame of reference. b2Vec2 p1 = b2Math.b2MulT(xf.q, input.p1 - xf.p); b2Vec2 p2 = b2Math.b2MulT(xf.q, input.p2 - xf.p); b2Vec2 d = p2 - p1; b2Vec2 v1 = m_vertex1; b2Vec2 v2 = m_vertex2; b2Vec2 e = v2 - v1; b2Vec2 normal = new b2Vec2(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 = b2Math.b2Dot(normal, v1 - p1); float denominator = b2Math.b2Dot(normal, d); if (denominator == 0.0f) { return false; } float t = numerator / denominator; if (t < 0.0f || input.maxFraction < t) { return false; } b2Vec2 q = p1 + t * d; // q = v1 + s * r // s = dot(q - v1, r) / dot(r, r) b2Vec2 r = v2 - v1; float rr = b2Math.b2Dot(r, r); if (rr == 0.0f) { return false; } float s = b2Math.b2Dot(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; }
public override b2AABB ComputeAABB(b2Transform xf, int childIndex) { int i1 = childIndex; int i2 = childIndex + 1; if (i2 == m_count) { i2 = 0; } b2Vec2 v1 = b2Math.b2Mul(xf, m_vertices[i1]); b2Vec2 v2 = b2Math.b2Mul(xf, m_vertices[i2]); b2AABB aabb = new b2AABB(); aabb.lowerBound = b2Math.b2Min(v1, v2); aabb.upperBound = b2Math.b2Max(v1, v2); return (aabb); }
/// Compute the collision manifold between two circles. public static void b2CollideCircles(ref b2Manifold manifold, b2CircleShape circleA, ref b2Transform xfA, b2CircleShape circleB, ref b2Transform xfB) { manifold.pointCount = 0; b2Vec2 pA = b2Math.b2Mul(xfA, circleA.Position); b2Vec2 pB = b2Math.b2Mul(xfB, circleB.Position); b2Vec2 d = pB - pA; float distSqr = b2Math.b2Dot(d, d); float rA = circleA.Radius, rB = circleB.Radius; float radius = rA + rB; if (distSqr > radius * radius) { return; } manifold.type = b2ManifoldType.e_circles; manifold.localPoint = circleA.Position; manifold.localNormal.SetZero(); manifold.pointCount = 1; manifold.points[0].localPoint = circleB.Position; manifold.points[0].id = b2ContactFeature.Zero; }
/// Cast a ray against a child shape. /// @param output the ray-cast results. /// @param input the ray-cast input parameters. /// @param transform the transform to be applied to the shape. /// @param childIndex the child shape index public abstract bool RayCast(out b2RayCastOutput output, b2RayCastInput input, b2Transform transform, int childIndex);
/// Compute the collision manifold between a polygon and a circle. public static void b2CollidePolygonAndCircle(ref b2Manifold manifold, b2PolygonShape polygonA, ref b2Transform xfA, b2CircleShape circleB, ref b2Transform xfB) { manifold.pointCount = 0; // Compute circle position in the frame of the polygon. b2Vec2 c = b2Math.b2Mul(xfB, circleB.Position); b2Vec2 cLocal = b2Math.b2MulT(xfA, c); // Find the min separating edge. int normalIndex = 0; float separation = -b2Settings.b2_maxFloat; float radius = polygonA.Radius + circleB.Radius; int vertexCount = polygonA.VertexCount; b2Vec2[] vertices = polygonA.Vertices; b2Vec2[] normals = polygonA.Normals; for (int i = 0; i < vertexCount; ++i) { float s = b2Math.b2Dot(normals[i], cLocal - vertices[i]); if (s > radius) { // Early out. return; } if (s > separation) { separation = s; normalIndex = i; } } // Vertices that subtend the incident face. int vertIndex1 = normalIndex; int vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0; b2Vec2 v1 = vertices[vertIndex1]; b2Vec2 v2 = vertices[vertIndex2]; // If the center is inside the polygon ... if (separation < b2Settings.b2_epsilon) { manifold.pointCount = 1; manifold.type = b2ManifoldType.e_faceA; manifold.localNormal = normals[normalIndex]; manifold.localPoint = 0.5f * (v1 + v2); manifold.points[0].localPoint = circleB.Position; manifold.points[0].id = b2ContactFeature.Zero; return; } // Compute barycentric coordinates float u1 = b2Math.b2Dot(cLocal - v1, v2 - v1); float u2 = b2Math.b2Dot(cLocal - v2, v1 - v2); if (u1 <= 0.0f) { if (b2Math.b2DistanceSquared(cLocal, v1) > radius * radius) { return; } manifold.pointCount = 1; manifold.type = b2ManifoldType.e_faceA; manifold.localNormal = cLocal - v1; manifold.localNormal.Normalize(); manifold.localPoint = v1; manifold.points[0].localPoint = circleB.Position; manifold.points[0].id = b2ContactFeature.Zero; } else if (u2 <= 0.0f) { if (b2Math.b2DistanceSquared(cLocal, v2) > radius * radius) { return; } manifold.pointCount = 1; manifold.type = b2ManifoldType.e_faceA; manifold.localNormal = cLocal - v2; manifold.localNormal.Normalize(); manifold.localPoint = v2; manifold.points[0].localPoint = circleB.Position; manifold.points[0].id = b2ContactFeature.Zero; } else { b2Vec2 faceCenter = 0.5f * (v1 + v2); separation = b2Math.b2Dot(cLocal - faceCenter, normals[vertIndex1]); if (separation > radius) { return; } manifold.pointCount = 1; manifold.type = b2ManifoldType.e_faceA; manifold.localNormal = normals[vertIndex1]; manifold.localPoint = faceCenter; manifold.points[0].localPoint = circleB.Position; manifold.points[0].id = b2ContactFeature.Zero; } }
/// Given a transform, compute the associated axis aligned bounding box for a child shape. /// @param aabb returns the axis aligned box. /// @param xf the world transform of the shape. /// @param childIndex the child shape public abstract b2AABB ComputeAABB(b2Transform xf, int childIndex);
/// Compute the collision manifold between two polygons. // Find edge normal of max separation on A - return if separating axis is found // Find edge normal of max separation on B - return if separation axis is found // Choose reference edge as min(minA, minB) // Find incident edge // Clip // The normal points from 1 to 2 public static void b2CollidePolygons(ref b2Manifold manifold, b2PolygonShape polyA, ref b2Transform xfA, b2PolygonShape polyB, ref b2Transform xfB) { manifold.pointCount = 0; float totalRadius = polyA.Radius + polyB.Radius; int edgeA = 0; float separationA = b2FindMaxSeparation(out edgeA, polyA, ref xfA, polyB, ref xfB); if (separationA > totalRadius) return; int edgeB = 0; float separationB = b2FindMaxSeparation(out edgeB, polyB, ref xfB, polyA, ref xfA); if (separationB > totalRadius) return; b2PolygonShape poly1; // reference polygon b2PolygonShape poly2; // incident polygon b2Transform xf1, xf2; int edge1; // reference edge byte flip; const float k_relativeTol = 0.98f; const float k_absoluteTol = 0.001f; if (separationB > k_relativeTol * separationA + k_absoluteTol) { poly1 = polyB; poly2 = polyA; xf1 = xfB; xf2 = xfA; edge1 = edgeB; manifold.type = b2ManifoldType.e_faceB; flip = 1; } else { poly1 = polyA; poly2 = polyB; xf1 = xfA; xf2 = xfB; edge1 = edgeA; manifold.type = b2ManifoldType.e_faceA; flip = 0; } b2ClipVertex[] incidentEdge = new b2ClipVertex[2]; b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2); int count1 = poly1.VertexCount; b2Vec2[] vertices1 = poly1.Vertices; int iv1 = edge1; int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0; b2Vec2 v11 = vertices1[iv1]; b2Vec2 v12 = vertices1[iv2]; b2Vec2 localTangent = v12 - v11; localTangent.Normalize(); b2Vec2 localNormal = localTangent.UnitCross(); // b2Math.b2Cross(localTangent, 1.0f); b2Vec2 planePoint = 0.5f * (v11 + v12); b2Vec2 tangent = b2Math.b2Mul(xf1.q, localTangent); b2Vec2 normal = tangent.UnitCross(); // b2Math.b2Cross(tangent, 1.0f); v11 = b2Math.b2Mul(xf1, v11); v12 = b2Math.b2Mul(xf1, v12); // Face offset. float frontOffset = b2Math.b2Dot(ref normal, ref v11); // Side offsets, extended by polytope skin thickness. float sideOffset1 = -b2Math.b2Dot(ref tangent, ref v11) + totalRadius; float sideOffset2 = b2Math.b2Dot(ref tangent, ref v12) + totalRadius; // Clip incident edge against extruded edge1 side edges. b2ClipVertex[] clipPoints1 = new b2ClipVertex[2]; b2ClipVertex[] clipPoints2 = new b2ClipVertex[2]; int np; // Clip to box side 1 np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, (byte)iv1); if (np < 2) return; // Clip to negative box side 1 np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, (byte)iv2); if (np < 2) { return; } // Now clipPoints2 contains the clipped points. manifold.localNormal = localNormal; manifold.localPoint = planePoint; int pointCount = 0; for (int i = 0; i < b2Settings.b2_maxManifoldPoints; ++i) { float separation = b2Math.b2Dot(ref normal, ref clipPoints2[i].v) - frontOffset; if (separation <= totalRadius) { b2ManifoldPoint cp = manifold.points[pointCount]; cp.localPoint = b2Math.b2MulT(xf2, clipPoints2[i].v); cp.id = clipPoints2[i].id; if (flip != 0) { // Swap features b2ContactFeature cf = cp.id; cp.id.indexA = cf.indexB; cp.id.indexB = cf.indexA; cp.id.typeA = cf.typeB; cp.id.typeB = cf.typeA; } manifold.points[pointCount] = cp; ++pointCount; } } manifold.pointCount = pointCount; }
public b2PositionSolverManifold(ref b2ContactPositionConstraint pc, ref b2Transform xfA, ref b2Transform xfB, int index) { Debug.Assert(pc.pointCount > 0); switch (pc.type) { case b2ManifoldType.e_circles: { b2Vec2 pointA = b2Math.b2Mul(xfA, pc.localPoint); b2Vec2 pointB = b2Math.b2Mul(xfB, pc.localPoints[0]); normal = pointB - pointA; normal.Normalize(); point = 0.5f * (pointA + pointB); separation = b2Math.b2Dot(pointB - pointA, normal) - pc.radiusA - pc.radiusB; } break; case b2ManifoldType.e_faceA: { normal = b2Math.b2Mul(xfA.q, pc.localNormal); b2Vec2 planePoint = b2Math.b2Mul(xfA, pc.localPoint); b2Vec2 clipPoint = b2Math.b2Mul(xfB, pc.localPoints[index]); separation = b2Math.b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; } break; case b2ManifoldType.e_faceB: { normal = b2Math.b2Mul(xfB.q, pc.localNormal); b2Vec2 planePoint = b2Math.b2Mul(xfB, pc.localPoint); b2Vec2 clipPoint = b2Math.b2Mul(xfA, pc.localPoints[index]); separation = b2Math.b2Dot(clipPoint - planePoint, normal) - pc.radiusA - pc.radiusB; point = clipPoint; // Ensure normal points from A to B normal = -normal; } break; } }