/// <summary> /// A fallback for handling degenerate "Star of David" cases. /// </summary> private static void FindVertsFallback( VoltPolygon poly1, VoltPolygon poly2, Vector2 normal, float penetration, Manifold manifold) { for (int i = 0; i < poly1.countWorld; i++) { Vector2 vertex = poly1.worldVertices[i]; if (poly2.ContainsPointPartial(vertex, normal) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) { return; } } } for (int i = 0; i < poly2.countWorld; i++) { Vector2 vertex = poly2.worldVertices[i]; if (poly1.ContainsPointPartial(vertex, -normal) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) { return; } } } }
private static bool FindMinSepAxis( VoltPolygon poly1, VoltPolygon poly2, out Axis axis) { axis = new Axis(Vector2.zero, float.NegativeInfinity); for (int i = 0; i < poly1.countWorld; i++) { Axis a = poly1.worldAxes[i]; float min = float.PositiveInfinity; for (int j = 0; j < poly2.countWorld; j++) { Vector2 v = poly2.worldVertices[j]; min = Mathf.Min(min, Vector2.Dot(a.Normal, v)); } min -= a.Width; if (min > 0) { return(false); } if (min > axis.Width) { axis = new Axis(a.Normal, min); } } return(true); }
private static bool FindMinSepAxis( VoltPolygon poly1, VoltPolygon poly2, out Axis axis) { axis = new Axis(VoltVector2.zero, Fix64.MinValue); for (int i = 0; i < poly1.countWorld; i++) { Axis a = poly1.worldAxes[i]; Fix64 min = Fix64.MaxValue; for (int j = 0; j < poly2.countWorld; j++) { VoltVector2 v = poly2.worldVertices[j]; min = VoltMath.Min(min, VoltVector2.Dot(a.Normal, v)); } min -= a.Width; if (min > Fix64.Zero) { return(false); } if (min > axis.Width) { axis = new Axis(a.Normal, min); } } return(true); }
/// <summary> /// Returns the index of the axis with the max circle penetration depth. /// Breaks out if a separating axis is found between the two shapes. /// Outputs the penetration depth of the circle in the axis (if any). /// </summary> internal static int FindAxisMaxPenetration( VoltVector2 origin, Fix64 radius, VoltPolygon poly, out Fix64 penetration) { int index = 0; int found = 0; penetration = Fix64.MinValue; for (int i = 0; i < poly.countWorld; i++) { Axis axis = poly.worldAxes[i]; Fix64 dot = VoltVector2.Dot(axis.Normal, origin); Fix64 dist = dot - axis.Width - radius; if (dist > Fix64.Zero) { return(-1); } if (dist > penetration) { penetration = dist; found = index; } index++; } return(found); }
private static Manifold Polygon_Polygon( VoltWorld world, VoltPolygon polyA, VoltPolygon polyB) { Axis a1, a2; if (Collision.FindMinSepAxis(polyA, polyB, out a1) == false) { return(null); } if (Collision.FindMinSepAxis(polyB, polyA, out a2) == false) { return(null); } // We will use poly1's axis, so we may need to swap if (a2.Width > a1.Width) { VoltUtil.Swap(ref polyA, ref polyB); VoltUtil.Swap(ref a1, ref a2); } // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, polyA, polyB); Collision.FindVerts(polyA, polyB, a1.Normal, a1.Width, manifold); return(manifold); }
/// <summary> /// Returns the index of the axis with the max circle penetration depth. /// Breaks out if a separating axis is found between the two shapes. /// Outputs the penetration depth of the circle in the axis (if any). /// </summary> internal static int FindAxisMaxPenetration( Vector2 origin, float radius, VoltPolygon poly, out float penetration) { int index = 0; int found = 0; penetration = float.NegativeInfinity; for (int i = 0; i < poly.countWorld; i++) { Axis axis = poly.worldAxes[i]; float dot = Vector2.Dot(axis.Normal, origin); float dist = dot - axis.Width - radius; if (dist > 0) { return(-1); } if (dist > penetration) { penetration = dist; found = index; } index++; } return(found); }
public override VoltShape PrepareShape(VoltWorld world) { Vector2[] vertices = new Vector2[this.points.Length]; for (int i = 0; i < this.points.Length; i++) vertices[i] = this.points[i].position; this.shape = world.CreatePolygonWorldSpace( vertices, this.density); return this.shape; }
/// <summary> /// Creates a new polygon shape from body-space vertices. /// </summary> public VoltPolygon CreatePolygonBodySpace( VoltVector2[] bodyVertices, Fix64 density, Fix64 friction, Fix64 restitution) { VoltPolygon polygon = (VoltPolygon)this.polygonPool.Allocate(); polygon.InitializeFromBodyVertices( bodyVertices, density, friction, restitution); return(polygon); }
protected override void ApplyBodyPosition() { for (int i = 0; i < this.countWorld; i++) { this.worldVertices[i] = this.Body.BodyToWorldPointCurrent(this.bodyVertices[i]); this.worldAxes[i] = this.Body.BodyToWorldAxisCurrent(this.bodyAxes[i]); } this.worldSpaceAABB = VoltPolygon.ComputeBounds(this.worldVertices, this.countWorld); }
/// <summary> /// Creates a new polygon shape from body-space vertices. /// </summary> public VoltPolygon CreatePolygonBodySpace( Vector2[] bodyVertices, float density = VoltConfig.DEFAULT_DENSITY, float friction = VoltConfig.DEFAULT_FRICTION, float restitution = VoltConfig.DEFAULT_RESTITUTION) { VoltPolygon polygon = (VoltPolygon)this.polygonPool.Allocate(); polygon.InitializeFromBodyVertices( bodyVertices, density, friction, restitution); return(polygon); }
/// <summary> /// Creates a new polygon shape from body-space vertices. /// </summary> public VoltPolygon CreatePolygonBodySpace( TSVector2[] bodyVertices, FP?density = null, FP?friction = null, FP?restitution = null) { VoltPolygon polygon = (VoltPolygon)this.polygonPool.Allocate(); polygon.InitializeFromBodyVertices( bodyVertices, density ?? VoltConfig.DEFAULT_DENSITY, friction ?? VoltConfig.DEFAULT_FRICTION, restitution ?? VoltConfig.DEFAULT_RESTITUTION); return(polygon); }
internal void InitializeFromWorldVertices( Vector2[] vertices, float density, float friction, float restitution) { base.Initialize(density, friction, restitution); this.UpdateArrays(vertices.Length); this.countWorld = vertices.Length; Array.Copy(vertices, this.worldVertices, this.countWorld); VoltPolygon.ComputeAxes(vertices, this.countWorld, ref this.worldAxes); this.worldSpaceAABB = VoltPolygon.ComputeBounds(vertices, this.countWorld); this.countBody = 0; // Needs to be set on metric compute }
private static Manifold Circle_Polygon( VoltWorld world, VoltCircle circ, VoltPolygon poly) { // Get the axis on the polygon closest to the circle's origin float penetration; int index = Collision.FindAxisMaxPenetration( circ.worldSpaceOrigin, circ.radius, poly, out penetration); if (index < 0) { return(null); } Vector2 a, b; poly.GetEdge(index, out a, out b); Axis axis = poly.GetWorldAxis(index); // If the circle is past one of the two vertices, check it like // a circle-circle intersection where the vertex has radius 0 float d = VoltMath.Cross(axis.Normal, circ.worldSpaceOrigin); if (d > VoltMath.Cross(axis.Normal, a)) { return(Collision.TestCircles(world, circ, poly, a, 0.0f)); } if (d < VoltMath.Cross(axis.Normal, b)) { return(Collision.TestCircles(world, circ, poly, b, 0.0f)); } // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, circ, poly); Vector2 pos = circ.worldSpaceOrigin - (circ.radius + penetration / 2) * axis.Normal; manifold.AddContact(pos, -axis.Normal, penetration); return(manifold); }
internal void InitializeFromBodyVertices( Vector2[] vertices, float density, float friction, float restitution) { base.Initialize(density, friction, restitution); this.UpdateArrays(vertices.Length); // World vertices will be computed on position update this.countWorld = vertices.Length; this.countBody = vertices.Length; Array.Copy(vertices, this.bodyVertices, vertices.Length); VoltPolygon.ComputeAxes(vertices, this.countBody, ref this.bodyAxes); this.bodySpaceAABB = VoltPolygon.ComputeBounds(vertices, this.countBody); }
/// <summary> /// Add contacts for penetrating vertices. Note that this does not handle /// cases where an overlap was detected, but no vertices fall inside the /// opposing polygon (like a Star of David). For this we have a fallback. /// /// See http://chipmunk-physics.googlecode.com/svn/trunk/src/cpCollision.c /// </summary> private static void FindVerts( VoltPolygon poly1, VoltPolygon poly2, Vector2 normal, float penetration, Manifold manifold) { bool found = false; for (int i = 0; i < poly1.countWorld; i++) { Vector2 vertex = poly1.worldVertices[i]; if (poly2.ContainsPoint(vertex) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) { return; } found = true; } } for (int i = 0; i < poly2.countWorld; i++) { Vector2 vertex = poly2.worldVertices[i]; if (poly1.ContainsPoint(vertex) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) { return; } found = true; } } // Fallback to check the degenerate "Star of David" case if (found == false) { FindVertsFallback(poly1, poly2, normal, penetration, manifold); } }
protected override void ComputeMetrics() { // If we were initialized with world points, we need to compute body if (this.countBody == 0) { // Compute body-space geometry data (only need to do this once) VoltPolygon.WorldToBody( this.Body, this.worldVertices, this.bodyVertices, this.countWorld); this.countBody = this.countWorld; VoltPolygon.ComputeAxes(this.bodyVertices, this.countBody, ref this.bodyAxes); this.bodySpaceAABB = VoltPolygon.ComputeBounds(this.bodyVertices, this.countBody); } this.Area = this.ComputeArea(); this.Mass = this.Area * this.Density * VoltConfig.AreaMassRatio; this.Inertia = this.ComputeInertia(); }
/// <summary> /// Returns the index of the axis with the max circle penetration depth. /// Breaks out if a separating axis is found between the two shapes. /// Outputs the penetration depth of the circle in the axis (if any). /// </summary> internal static int FindAxisMaxPenetration( Vector2 origin, float radius, VoltPolygon poly, out float penetration) { int index = 0; int found = 0; penetration = float.NegativeInfinity; for (int i = 0; i < poly.countWorld; i++) { Axis axis = poly.worldAxes[i]; float dot = Vector2.Dot(axis.Normal, origin); float dist = dot - axis.Width - radius; if (dist > 0) return -1; if (dist > penetration) { penetration = dist; found = index; } index++; } return found; }
private static Manifold Circle_Polygon( VoltWorld world, VoltCircle circ, VoltPolygon poly) { // Get the axis on the polygon closest to the circle's origin float penetration; int index = Collision.FindAxisMaxPenetration( circ.worldSpaceOrigin, circ.radius, poly, out penetration); if (index < 0) return null; Vector2 a, b; poly.GetEdge(index, out a, out b); Axis axis = poly.GetWorldAxis(index); // If the circle is past one of the two vertices, check it like // a circle-circle intersection where the vertex has radius 0 float d = VoltMath.Cross(axis.Normal, circ.worldSpaceOrigin); if (d > VoltMath.Cross(axis.Normal, a)) return Collision.TestCircles(world, circ, poly, a, 0.0f); if (d < VoltMath.Cross(axis.Normal, b)) return Collision.TestCircles(world, circ, poly, b, 0.0f); // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, circ, poly); Vector2 pos = circ.worldSpaceOrigin - (circ.radius + penetration / 2) * axis.Normal; manifold.AddContact(pos, -axis.Normal, penetration); return manifold; }
private static bool FindMinSepAxis( VoltPolygon poly1, VoltPolygon poly2, out Axis axis) { axis = new Axis(Vector2.zero, float.NegativeInfinity); for (int i = 0; i < poly1.countWorld; i++) { Axis a = poly1.worldAxes[i]; float min = float.PositiveInfinity; for (int j = 0; j < poly2.countWorld; j++) { Vector2 v = poly2.worldVertices[j]; min = Mathf.Min(min, Vector2.Dot(a.Normal, v)); } min -= a.Width; if (min > 0) return false; if (min > axis.Width) axis = new Axis(a.Normal, min); } return true; }
/// <summary> /// Add contacts for penetrating vertices. Note that this does not handle /// cases where an overlap was detected, but no vertices fall inside the /// opposing polygon (like a Star of David). For this we have a fallback. /// /// See http://chipmunk-physics.googlecode.com/svn/trunk/src/cpCollision.c /// </summary> private static void FindVerts( VoltPolygon poly1, VoltPolygon poly2, Vector2 normal, float penetration, Manifold manifold) { bool found = false; for (int i = 0; i < poly1.countWorld; i++) { Vector2 vertex = poly1.worldVertices[i]; if (poly2.ContainsPoint(vertex) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) return; found = true; } } for (int i = 0; i < poly2.countWorld; i++) { Vector2 vertex = poly2.worldVertices[i]; if (poly1.ContainsPoint(vertex) == true) { if (manifold.AddContact(vertex, normal, penetration) == false) return; found = true; } } // Fallback to check the degenerate "Star of David" case if (found == false) FindVertsFallback(poly1, poly2, normal, penetration, manifold); }
/// <summary> /// A fallback for handling degenerate "Star of David" cases. /// </summary> private static void FindVertsFallback( VoltPolygon poly1, VoltPolygon poly2, Vector2 normal, float penetration, Manifold manifold) { for (int i = 0; i < poly1.countWorld; i++) { Vector2 vertex = poly1.worldVertices[i]; if (poly2.ContainsPointPartial(vertex, normal) == true) if (manifold.AddContact(vertex, normal, penetration) == false) return; } for (int i = 0; i < poly2.countWorld; i++) { Vector2 vertex = poly2.worldVertices[i]; if (poly1.ContainsPointPartial(vertex, -normal) == true) if (manifold.AddContact(vertex, normal, penetration) == false) return; } }
private static Manifold Polygon_Polygon( VoltWorld world, VoltPolygon polyA, VoltPolygon polyB) { Axis a1, a2; if (Collision.FindMinSepAxis(polyA, polyB, out a1) == false) return null; if (Collision.FindMinSepAxis(polyB, polyA, out a2) == false) return null; // We will use poly1's axis, so we may need to swap if (a2.Width > a1.Width) { VoltUtil.Swap(ref polyA, ref polyB); VoltUtil.Swap(ref a1, ref a2); } // Build the collision Manifold Manifold manifold = world.AllocateManifold().Assign(world, polyA, polyB); Collision.FindVerts(polyA, polyB, a1.Normal, a1.Width, manifold); return manifold; }