/// <summary> /// Compute contact points for edge versus circle. /// This accounts for edge connectivity. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="transformA">The transform A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform B.</param> public static void CollideEdgeAndCircle(ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB) { manifold.PointCount = 0; // Compute circle in frame of edge Vector2 Q = MathUtils.MultiplyT(ref transformA, MathUtils.Multiply(ref transformB, ref circleB._position)); Vector2 A = edgeA.Vertex1, B = edgeA.Vertex2; Vector2 e = B - A; // Barycentric coordinates float u = Vector2.Dot(e, B - Q); float v = Vector2.Dot(e, Q - A); float radius = edgeA.Radius + circleB.Radius; ContactFeature cf; cf.IndexB = 0; cf.TypeB = (byte)ContactFeatureType.Vertex; Vector2 P, d; // Region A if (v <= 0.0f) { P = A; d = Q - P; float dd; Vector2.Dot(ref d, ref d, out dd); if (dd > radius * radius) { return; } // Is there an edge connected to A? if (edgeA.HasVertex0) { Vector2 A1 = edgeA.Vertex0; Vector2 B1 = A; Vector2 e1 = B1 - A1; float u1 = Vector2.Dot(e1, B1 - Q); // Is the circle in Region AB of the previous edge? if (u1 > 0.0f) { return; } } cf.IndexA = 0; cf.TypeA = (byte)ContactFeatureType.Vertex; manifold.PointCount = 1; manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; ManifoldPoint mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB.Position; manifold.Points[0] = mp; return; } // Region B if (u <= 0.0f) { P = B; d = Q - P; float dd; Vector2.Dot(ref d, ref d, out dd); if (dd > radius * radius) { return; } // Is there an edge connected to B? if (edgeA.HasVertex3) { Vector2 B2 = edgeA.Vertex3; Vector2 A2 = B; Vector2 e2 = B2 - A2; float v2 = Vector2.Dot(e2, Q - A2); // Is the circle in Region AB of the next edge? if (v2 > 0.0f) { return; } } cf.IndexA = 1; cf.TypeA = (byte)ContactFeatureType.Vertex; manifold.PointCount = 1; manifold.Type = ManifoldType.Circles; manifold.LocalNormal = Vector2.Zero; manifold.LocalPoint = P; ManifoldPoint mp = new ManifoldPoint(); mp.Id.Key = 0; mp.Id.Features = cf; mp.LocalPoint = circleB.Position; manifold.Points[0] = mp; return; } // Region AB float den; Vector2.Dot(ref e, ref e, out den); Debug.Assert(den > 0.0f); P = (1.0f / den) * (u * A + v * B); d = Q - P; float dd2; Vector2.Dot(ref d, ref d, out dd2); if (dd2 > radius * radius) { return; } Vector2 n = new Vector2(-e.Y, e.X); if (Vector2.Dot(n, Q - A) < 0.0f) { n = new Vector2(-n.X, -n.Y); } n.Normalize(); cf.IndexA = 0; cf.TypeA = (byte)ContactFeatureType.Face; manifold.PointCount = 1; manifold.Type = ManifoldType.FaceA; manifold.LocalNormal = n; manifold.LocalPoint = A; ManifoldPoint mp2 = new ManifoldPoint(); mp2.Id.Key = 0; mp2.Id.Features = cf; mp2.LocalPoint = circleB.Position; manifold.Points[0] = mp2; }
/// <summary> /// Compute contact points for edge versus circle. /// This accounts for edge connectivity. /// </summary> /// <param name="manifold">The manifold.</param> /// <param name="edgeA">The edge A.</param> /// <param name="transformA">The transform A.</param> /// <param name="circleB">The circle B.</param> /// <param name="transformB">The transform B.</param> public static void collideEdgeAndCircle( ref Manifold manifold, EdgeShape edgeA, ref Transform transformA, CircleShape circleB, ref Transform transformB ) { manifold.pointCount = 0; // Compute circle in frame of edge var Q = MathUtils.mulT( ref transformA, MathUtils.Mul( ref transformB, ref circleB._position ) ); Vector2 A = edgeA.vertex1, B = edgeA.vertex2; var e = B - A; // Barycentric coordinates var u = Vector2.Dot( e, B - Q ); var v = Vector2.Dot( e, Q - A ); var radius = edgeA.radius + circleB.radius; ContactFeature cf; cf.indexB = 0; cf.typeB = (byte)ContactFeatureType.Vertex; Vector2 P, d; // Region A if( v <= 0.0f ) { P = A; d = Q - P; float dd; Vector2.Dot( ref d, ref d, out dd ); if( dd > radius * radius ) return; // Is there an edge connected to A? if( edgeA.hasVertex0 ) { var A1 = edgeA.vertex0; var B1 = A; var e1 = B1 - A1; var u1 = Vector2.Dot( e1, B1 - Q ); // Is the circle in Region AB of the previous edge? if( u1 > 0.0f ) return; } cf.indexA = 0; cf.typeA = (byte)ContactFeatureType.Vertex; manifold.pointCount = 1; manifold.type = ManifoldType.Circles; manifold.localNormal = Vector2.Zero; manifold.localPoint = P; var mp = new ManifoldPoint(); mp.id.key = 0; mp.id.features = cf; mp.localPoint = circleB.position; manifold.points[0] = mp; return; } // Region B if( u <= 0.0f ) { P = B; d = Q - P; float dd; Vector2.Dot( ref d, ref d, out dd ); if( dd > radius * radius ) return; // Is there an edge connected to B? if( edgeA.hasVertex3 ) { var B2 = edgeA.vertex3; var A2 = B; var e2 = B2 - A2; var v2 = Vector2.Dot( e2, Q - A2 ); // Is the circle in Region AB of the next edge? if( v2 > 0.0f ) return; } cf.indexA = 1; cf.typeA = (byte)ContactFeatureType.Vertex; manifold.pointCount = 1; manifold.type = ManifoldType.Circles; manifold.localNormal = Vector2.Zero; manifold.localPoint = P; var mp = new ManifoldPoint(); mp.id.key = 0; mp.id.features = cf; mp.localPoint = circleB.position; manifold.points[0] = mp; return; } // Region AB float den; Vector2.Dot( ref e, ref e, out den ); Debug.Assert( den > 0.0f ); P = ( 1.0f / den ) * ( u * A + v * B ); d = Q - P; float dd2; Vector2.Dot( ref d, ref d, out dd2 ); if( dd2 > radius * radius ) return; var n = new Vector2( -e.Y, e.X ); if( Vector2.Dot( n, Q - A ) < 0.0f ) n = new Vector2( -n.X, -n.Y ); n.Normalize(); cf.indexA = 0; cf.typeA = (byte)ContactFeatureType.Face; manifold.pointCount = 1; manifold.type = ManifoldType.FaceA; manifold.localNormal = n; manifold.localPoint = A; ManifoldPoint mp2 = new ManifoldPoint(); mp2.id.key = 0; mp2.id.features = cf; mp2.localPoint = circleB.position; manifold.points[0] = mp2; }