void SetupDescriptorSetLayout(Pipeline pipeline, GraphicsDevice device)
{
FixedArray2 <VkDescriptorSetLayoutBinding> setLayoutBindings = new FixedArray2 <VkDescriptorSetLayoutBinding>(
// Binding 0 : Vertex shader uniform buffer
Initializers.descriptorSetLayoutBinding(
VkDescriptorType.UniformBuffer,
VkShaderStageFlags.Vertex | VkShaderStageFlags.Fragment,
0),
// Binding 1 : Fragment shader combined sampler
Initializers.descriptorSetLayoutBinding(
VkDescriptorType.CombinedImageSampler,
VkShaderStageFlags.Fragment,
1));
VkDescriptorSetLayoutCreateInfo descriptorLayout =
Initializers.descriptorSetLayoutCreateInfo(
&setLayoutBindings.First,
setLayoutBindings.Count);
Util.CheckResult(vkCreateDescriptorSetLayout(device.device, &descriptorLayout, null, out pipeline.descriptorSetLayout));
var dsl = pipeline.descriptorSetLayout;
VkPipelineLayoutCreateInfo pPipelineLayoutCreateInfo =
Initializers.pipelineLayoutCreateInfo(
&dsl,
1);
Util.CheckResult(vkCreatePipelineLayout(device.device, &pPipelineLayoutCreateInfo, null, out pipeline.pipelineLayout));
}
public void ComputeOldWorldManifold(out Vector2 normal, out IList <WorldPoint> points)
{
if (OldManifold.PointCount < 1)
{
normal = Vector2.Zero;
points = new FixedArray2 <WorldPoint>();
return;
}
var bodyA = Contact.FixtureA.Body;
var bodyB = Contact.FixtureB.Body;
var transformA = bodyA.Transform;
var transformB = bodyB.Transform;
var radiusA = Contact.FixtureA.Radius;
var radiusB = Contact.FixtureB.Radius;
FixedArray2 <WorldPoint> worldPoints;
OldManifold.ComputeWorldManifold(
transformA,
radiusA,
transformB,
radiusB,
out normal,
out worldPoints);
worldPoints.Count = OldManifold.PointCount;
points = worldPoints;
}
internal void Update(Contact c)
{
if (c != null)
{
if (contacts[0] == null)
{
contacts[0] = new ContactPoint2D();
}
WorldManifold worldManifold;
c.GetWorldManifold(out worldManifold);
System.Numerics.Vector2 normal = worldManifold.Normal;
FixedArray2 <System.Numerics.Vector2> points = worldManifold.Points;
if (this.bodyA == c.FixtureA.Body)
{
contacts[0].normal = -normal;
}
else
{
contacts[0].normal = normal;
}
contacts[0].point = points[0];
}
}
void SetupDescriptorSet(Pipeline pipeline, GraphicsDevice device, UniformBuffer uniformBuffer, Texture2D texture_colorMap)
{
var dsl = pipeline.descriptorSetLayout;
VkDescriptorSetAllocateInfo allocInfo =
Initializers.descriptorSetAllocateInfo(
pipeline.descriptorPool,
&dsl,
1);
Util.CheckResult(vkAllocateDescriptorSets(device.device, &allocInfo, out pipeline.descriptorSet));
VkDescriptorImageInfo texDescriptor =
Initializers.descriptorImageInfo(
texture_colorMap.sampler,
texture_colorMap.view,
VkImageLayout.General);
VkDescriptorBufferInfo temp = uniformBuffer.GetVkDescriptor();
FixedArray2 <VkWriteDescriptorSet> writeDescriptorSets = new FixedArray2 <VkWriteDescriptorSet>(
// Binding 0 : Vertex shader uniform buffer
Initializers.writeDescriptorSet(
pipeline.descriptorSet,
VkDescriptorType.UniformBuffer,
uniformBuffer.location,
&temp),
// Binding 1 : Color map
Initializers.writeDescriptorSet(
pipeline.descriptorSet,
VkDescriptorType.CombinedImageSampler,
1,
&texDescriptor));
vkUpdateDescriptorSets(device.device, (writeDescriptorSets.Count), ref writeDescriptorSets.First, 0, null);
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void getWorldManifold(out Vector2 normal, out FixedArray2 <Vector2> points)
{
var bodyA = fixtureA.body;
var bodyB = fixtureB.body;
var shapeA = fixtureA.shape;
var shapeB = fixtureB.shape;
ContactSolver.WorldManifold.initialize(ref manifold, ref bodyA._xf, shapeA.radius, ref bodyB._xf, shapeB.radius, out normal, out points);
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out FixedArray2 <Vector2> points)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points);
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out System.Numerics.Vector2 normal, out FixedArray2 <System.Numerics.Vector2> points)
{
var bodyA = FixtureA.Body;
var bodyB = FixtureB.Body;
var shapeA = FixtureA.Shape;
var shapeB = FixtureB.Shape;
ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf,
shapeB.Radius, out normal, out points);
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out FixedArray2 <Vector2> points)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
Collision.Collision.GetWorldManifold(ref Manifold, ref bodyA.Xf, shapeA.Radius, ref bodyB.Xf, shapeB.Radius,
out normal, out points);
}
/// Clipping for contact manifolds.
private static int ClipSegmentToLine(
out FixedArray2 <ClipVertex> vOut,
FixedArray2 <ClipVertex> vIn,
Vector2 normal,
float offset,
int vertexIndexA)
{
// Satisfy outs.
vOut = new FixedArray2 <ClipVertex>();
// Start with no output points
var numOut = 0;
// Calculate the distance of end points to the line
var distance0 = Vector2Util.Dot(ref normal, ref vIn.Item1.Vertex) - offset;
var distance1 = Vector2Util.Dot(ref normal, ref vIn.Item2.Vertex) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut.Item1 = vIn.Item1;
++numOut;
if (distance1 <= 0.0f)
{
vOut.Item2 = vIn.Item2;
++numOut;
}
}
else if (distance1 <= 0.0f)
{
vOut.Item1 = vIn.Item2;
++numOut;
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
System.Diagnostics.Debug.Assert(numOut == 1);
// Find intersection point of edge and plane
var interp = distance0 / (distance0 - distance1);
vOut.Item2.Vertex = vIn.Item1.Vertex + interp * (vIn.Item2.Vertex - vIn.Item1.Vertex);
// VertexA is hitting edgeB.
vOut.Item2.Id.Feature.IndexA = (byte)vertexIndexA;
vOut.Item2.Id.Feature.IndexB = vIn.Item1.Id.Feature.IndexB;
vOut.Item2.Id.Feature.TypeA = (byte)ContactFeature.FeatureType.Vertex;
vOut.Item2.Id.Feature.TypeB = (byte)ContactFeature.FeatureType.Face;
++numOut;
}
return(numOut);
}
protected virtual void SetupDescriptorPool(Pipeline pipeline, GraphicsDevice device)
{
// Example uses one ubo and one combined image sampler
FixedArray2 <VkDescriptorPoolSize> poolSizes = new FixedArray2 <VkDescriptorPoolSize>(
Initializers.descriptorPoolSize(VkDescriptorType.UniformBuffer, 1),
Initializers.descriptorPoolSize(VkDescriptorType.CombinedImageSampler, 1));
VkDescriptorPoolCreateInfo descriptorPoolInfo =
Initializers.descriptorPoolCreateInfo(
poolSizes.Count,
&poolSizes.First,
1);
Util.CheckResult(vkCreateDescriptorPool(device.device, &descriptorPoolInfo, null, out pipeline.descriptorPool));
}
public static void GetPointStates(out FixedArray2 <PointState> state1, out FixedArray2 <PointState> state2,
ref Manifold manifold1, ref Manifold manifold2)
{
state1 = new FixedArray2 <PointState>();
state2 = new FixedArray2 <PointState>();
for (var i = 0; i < Settings.MaxManifoldPoints; ++i)
{
state1[i] = PointState.Null;
state2[i] = PointState.Null;
}
// Detect persists and removes.
for (var i = 0; i < manifold1.PointCount; ++i)
{
var id = manifold1.Points[i].Id;
state1[i] = PointState.Remove;
for (var j = 0; j < manifold2.PointCount; ++j)
{
if (manifold2.Points[j].Id.Key == id.Key)
{
state1[i] = PointState.Persist;
break;
}
}
}
// Detect persists and adds.
for (var i = 0; i < manifold2.PointCount; ++i)
{
var id = manifold2.Points[i].Id;
state2[i] = PointState.Add;
for (var j = 0; j < manifold1.PointCount; ++j)
{
if (manifold1.Points[j].Id.Key == id.Key)
{
state2[i] = PointState.Persist;
break;
}
}
}
}
/// <summary>
/// Finds the incident edge using the specified c
/// </summary>
/// <param name="c">The </param>
/// <param name="poly1">The poly</param>
/// <param name="xf1">The xf</param>
/// <param name="edge1">The edge</param>
/// <param name="poly2">The poly</param>
/// <param name="xf2">The xf</param>
private static void FindIncidentEdge(out FixedArray2 <ClipVertex> c, PolygonShape poly1, ref Transform xf1,
int edge1, PolygonShape poly2, ref Transform xf2)
{
Vertices normals1 = poly1.NormalsPrivate;
int count2 = poly2.VerticesPrivate.Count;
Vertices vertices2 = poly2.VerticesPrivate;
Vertices normals2 = poly2.NormalsPrivate;
Debug.Assert(0 <= edge1 && edge1 < poly1.VerticesPrivate.Count);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = MathUtils.MulT(ref xf2.Q, MathUtils.Mul(ref xf1.Q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = MathConstants.MaxFloat;
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 FixedArray2 <ClipVertex>();
c.Value0.V = MathUtils.Mul(ref xf2, vertices2[i1]);
c.Value0.Id.ContactFeature.IndexA = (byte)edge1;
c.Value0.Id.ContactFeature.IndexB = (byte)i1;
c.Value0.Id.ContactFeature.TypeA = ContactFeatureType.Face;
c.Value0.Id.ContactFeature.TypeB = ContactFeatureType.Vertex;
c.Value1.V = MathUtils.Mul(ref xf2, vertices2[i2]);
c.Value1.Id.ContactFeature.IndexA = (byte)edge1;
c.Value1.Id.ContactFeature.IndexB = (byte)i2;
c.Value1.Id.ContactFeature.TypeA = ContactFeatureType.Face;
c.Value1.Id.ContactFeature.TypeB = ContactFeatureType.Vertex;
}
/// <summary>Clipping for contact manifolds.</summary>
/// <param name="vOut">The v out.</param>
/// <param name="vIn">The v in.</param>
/// <param name="normal">The normal.</param>
/// <param name="offset">The offset.</param>
/// <param name="vertexIndexA">The vertex index A.</param>
/// <returns></returns>
internal static int ClipSegmentToLine(out FixedArray2 <ClipVertex> vOut, ref FixedArray2 <ClipVertex> vIn,
Vector2 normal, float offset, int vertexIndexA)
{
vOut = new FixedArray2 <ClipVertex>();
// Start with no output points
int count = 0;
// Calculate the distance of end points to the line
float distance0 = Vector2.Dot(normal, vIn.Value0.V) - offset;
float distance1 = Vector2.Dot(normal, vIn.Value1.V) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f)
{
vOut[count++] = vIn.Value0;
}
if (distance1 <= 0.0f)
{
vOut[count++] = vIn.Value1;
}
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
ClipVertex cv = vOut[count];
cv.V = vIn.Value0.V + interp * (vIn.Value1.V - vIn.Value0.V);
// VertexA is hitting edgeB.
cv.Id.ContactFeature.IndexA = (byte)vertexIndexA;
cv.Id.ContactFeature.IndexB = vIn.Value0.Id.ContactFeature.IndexB;
cv.Id.ContactFeature.TypeA = ContactFeatureType.Vertex;
cv.Id.ContactFeature.TypeB = ContactFeatureType.Face;
vOut[count] = cv;
++count;
}
return(count);
}
internal void Initialize(Fixture fixtureA, int indexA, Fixture fixtureB, int indexB)
{
Flags = ContactFlag.EnabledFlag;
FixtureA = fixtureA;
FixtureB = fixtureB;
ChildIndexA = indexA;
ChildIndexB = indexB;
ToiCount = 0;
Friction = MixFriction(FixtureA.Friction, FixtureB.Friction);
Restitution = MixRestitution(FixtureA.Restitution, FixtureB.Restitution);
TangentSpeed = 0.0f;
Manifold = new Manifold()
{
Points = FixedArray2 <ManifoldPoint> .Create()
};
}
/// <summary>
/// Begin renderpass and clear the color and depth of the supplied framebuffer.
/// </summary>
/// <param name="renderPass"></param>
/// <param name="frameBuffer"></param>
/// <param name="clearColor"></param>
/// <param name="clearDepth"></param>
public void BeginRenderPassClearColorDepth(RenderPass renderPass, FrameBuffer frameBuffer, VkClearColorValue clearColor, VkClearDepthStencilValue clearDepth, bool useSecondaryCommandBuffers)
{
CheckBegun();
CheckNotInRenderPass();
FixedArray2 <VkClearValue> clearValues = new FixedArray2 <VkClearValue>();
clearValues.First.color = clearColor;
clearValues.Second.depthStencil = clearDepth;
VkRenderPassBeginInfo renderPassBeginInfo = Initializers.renderPassBeginInfo();
renderPassBeginInfo.renderPass = renderPass.vkRenderPass;
renderPassBeginInfo.renderArea.offset.x = 0;
renderPassBeginInfo.renderArea.offset.y = 0;
renderPassBeginInfo.renderArea.extent.width = frameBuffer.swapchain.width;
renderPassBeginInfo.renderArea.extent.height = frameBuffer.swapchain.height;
renderPassBeginInfo.clearValueCount = 2;
renderPassBeginInfo.pClearValues = &clearValues.First;
renderPassBeginInfo.framebuffer = frameBuffer.vkFrameBuffer;
renderPassUseSecondaryBuffers = useSecondaryCommandBuffers;
VkSubpassContents subPassContents = useSecondaryCommandBuffers
? VkSubpassContents.SecondaryCommandBuffers
: VkSubpassContents.Inline;
vkCmdBeginRenderPass(vkCmd, &renderPassBeginInfo, subPassContents);
VkViewport viewport = Initializers.viewport((float)frameBuffer.swapchain.width, (float)frameBuffer.swapchain.height, 0.0f, 1.0f);
vkCmdSetViewport(vkCmd, 0, 1, &viewport);
VkRect2D scissor = Initializers.rect2D(frameBuffer.swapchain.width, frameBuffer.swapchain.height, 0, 0);
vkCmdSetScissor(vkCmd, 0, 1, &scissor);
inRenderPass = true;
}
static void FindIncidentEdge(ref FixedArray2<ClipVertex> c, PolygonShape poly1, int edge1, PolygonShape poly2)
{
int count1 = poly1._vertexCount;
int count2 = poly2._vertexCount;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = poly1._normals[edge1];
// Find the incident edge on poly2.
int index = 0;
float minDot = float.MaxValue;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, poly2._normals[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;
ClipVertex ctemp = new ClipVertex();
ctemp.v = poly2._vertices[i1];
ctemp.id.Features.indexA = (byte)edge1;
ctemp.id.Features.indexB = (byte)i1;
ctemp.id.Features.typeA = (byte)ContactFeatureType.Face;
ctemp.id.Features.typeB = (byte)ContactFeatureType.Vertex;
c[0] = ctemp;
ctemp.v = poly2._vertices[i2];
ctemp.id.Features.indexA = (byte)edge1;
ctemp.id.Features.indexB = (byte)i2;
ctemp.id.Features.typeA = (byte)ContactFeatureType.Face;
ctemp.id.Features.typeB = (byte)ContactFeatureType.Vertex;
c[1] = ctemp;
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="xfA">The transform for A.</param>
/// <param name="radiusA">The radius for A.</param>
/// <param name="xfB">The transform for B.</param>
/// <param name="radiusB">The radius for B.</param>
/// <param name="normal">World vector pointing from A to B</param>
/// <param name="points">Torld contact point (point of intersection).</param>
public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2<Vector2> points)
{
normal = Vector2.Zero;
points = new FixedArray2<Vector2>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new Vector2(1.0f, 0.0f);
Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
public static Manifold CalculateContactPoints(this Contact contact, out Vector2 worldNormal, out FixedArray2 <Vector2> worldPoints)
{
// Farseer does not generate manifolds for sensors
// => taken from farseer source code, performing manifold collision calculation for sensors
if (!(contact.FixtureA.IsSensor || contact.FixtureB.IsSensor))
{
contact.GetWorldManifold(out worldNormal, out worldPoints);
return(contact.Manifold);
}
Shape shapeA = contact.FixtureA.Shape;
Transform transformA;
contact.FixtureA.Body.GetTransform(out transformA);
Shape shapeB = contact.FixtureB.Shape;
Transform transformB;
contact.FixtureB.Body.GetTransform(out transformB);
Manifold manifold = new Manifold();
EdgeShape edgeShape;
switch (_registers[(int)shapeA.ShapeType, (int)shapeB.ShapeType])
{
case ContactType.Polygon:
Collision.CollidePolygons(ref manifold, (PolygonShape)shapeA, ref transformA, (PolygonShape)shapeB, ref transformB);
break;
case ContactType.PolygonAndCircle:
Collision.CollidePolygonAndCircle(ref manifold, (PolygonShape)shapeA, ref transformA, (CircleShape)shapeB, ref transformB);
break;
case ContactType.EdgeAndCircle:
Collision.CollideEdgeAndCircle(ref manifold, (EdgeShape)shapeA, ref transformA, (CircleShape)shapeB, ref transformB);
break;
case ContactType.EdgeAndPolygon:
Collision.CollideEdgeAndPolygon(ref manifold, (EdgeShape)shapeA, ref transformA, (PolygonShape)shapeB, ref transformB);
break;
case ContactType.ChainAndCircle:
ChainShape chain = (ChainShape)shapeA;
edgeShape = chain.GetChildEdge(contact.ChildIndexA);
Collision.CollideEdgeAndCircle(ref manifold, edgeShape, ref transformA, (CircleShape)shapeB, ref transformB);
break;
case ContactType.ChainAndPolygon:
ChainShape loop2 = (ChainShape)shapeA;
edgeShape = loop2.GetChildEdge(contact.ChildIndexA);
Collision.CollideEdgeAndPolygon(ref manifold, edgeShape, ref transformA, (PolygonShape)shapeB, ref transformB);
break;
case ContactType.Circle:
Collision.CollideCircles(ref manifold, (CircleShape)shapeA, ref transformA, (CircleShape)shapeB, ref transformB);
break;
}
ContactSolver.WorldManifold.Initialize(ref manifold, ref transformA, shapeA.Radius, ref transformB, shapeB.Radius, out worldNormal, out worldPoints);
return(manifold);
}
/// <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
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out FixedArray2<Vector2> points)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
ContactSolver.WorldManifold.Initialize(ref Manifold, ref bodyA._xf, shapeA.Radius, ref bodyB._xf, shapeB.Radius, out normal, out points);
}
public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
_xf = MathUtils.MulT(xfA, xfB);
_centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid);
_v0 = edgeA.Vertex0;
_v1 = edgeA._vertex1;
_v2 = edgeA._vertex2;
_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
Vector2 edge1 = _v2 - _v1;
edge1.Normalize();
_normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(_normal1, _centroidB - _v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vector2 edge0 = _v1 - _v0;
edge0.Normalize();
_normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(_normal0, _centroidB - _v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vector2 edge2 = _v3 - _v2;
edge2.Normalize();
_normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(_normal2, _centroidB - _v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
}
else if (convex1)
{
_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal1;
}
}
else if (convex2)
{
_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal0;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
}
else
{
_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = _normal1;
}
}
}
else
{
_front = offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
}
// Get polygonB in frameA
_polygonB.Count = polygonB.Vertices.Count;
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
_polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]);
_polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]);
}
_radius = 2.0f * Settings.PolygonRadius;
manifold.PointCount = 0;
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
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;
}
FixedArray2<ClipVertex> ie = new FixedArray2<ClipVertex>();
ReferenceFace rf;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]);
for (int i = 1; i < _polygonB.Count; ++i)
{
float value = Vector2.Dot(_normal, _polygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0;
ClipVertex c0 = ie[0];
c0.V = _polygonB.Vertices[i1];
c0.ID.Features.IndexA = 0;
c0.ID.Features.IndexB = (byte)i1;
c0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
c0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
ie[0] = c0;
ClipVertex c1 = ie[1];
c1.V = _polygonB.Vertices[i2];
c1.ID.Features.IndexA = 0;
c1.ID.Features.IndexB = (byte)i2;
c1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
c1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
ie[1] = c1;
if (_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = _v1;
rf.v2 = _v2;
rf.normal = _normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = _v2;
rf.v2 = _v1;
rf.normal = -_normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ClipVertex c0 = ie[0];
c0.V = _v1;
c0.ID.Features.IndexA = 0;
c0.ID.Features.IndexB = (byte)primaryAxis.Index;
c0.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
c0.ID.Features.TypeB = (byte)ContactFeatureType.Face;
ie[0] = c0;
ClipVertex c1 = ie[1];
c1.V = _v2;
c1.ID.Features.IndexA = 0;
c1.ID.Features.IndexB = (byte)primaryAxis.Index;
c1.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
c1.ID.Features.TypeB = (byte)ContactFeatureType.Face;
ie[1] = c1;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0;
rf.v1 = _polygonB.Vertices[rf.i1];
rf.v2 = _polygonB.Vertices[rf.i2];
rf.normal = _polygonB.Normals[rf.i1];
}
rf.sideNormal1 = new Vector2(rf.normal.Y, -rf.normal.X);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = Vector2.Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = Vector2.Dot(rf.sideNormal2, rf.v2);
// 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 ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = rf.normal;
manifold.LocalPoint = rf.v1;
}
else
{
manifold.LocalNormal = polygonB.Normals[rf.i1];
manifold.LocalPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(rf.normal, clipPoints2[i].V - rf.v1);
if (separation <= _radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.Features.TypeA = clipPoints2[i].ID.Features.TypeB;
cp.Id.Features.TypeB = clipPoints2[i].ID.Features.TypeA;
cp.Id.Features.IndexA = clipPoints2[i].ID.Features.IndexB;
cp.Id.Features.IndexB = clipPoints2[i].ID.Features.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="transformA">The transform for A.</param>
/// <param name="radiusA">The radius for A.</param>
/// <param name="transformB">The transform for B.</param>
/// <param name="radiusB">The radius for B.</param>
/// <param name="normal">World vector pointing from A to B</param>
/// <param name="points">Torld contact point (point of intersection).</param>
public static void GetWorldManifold(ref Manifold manifold,
ref Transform transformA, float radiusA,
ref Transform transformB, float radiusB, out Vector2 normal,
out FixedArray2<Vector2> points)
{
points = new FixedArray2<Vector2>();
normal = Vector2.Zero;
if (manifold.PointCount == 0)
{
normal = Vector2.UnitY;
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
Vector2 tmp = manifold.Points[0].LocalPoint;
float pointAx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
transformA.R.Col2.X * manifold.LocalPoint.Y;
float pointAy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
transformA.R.Col2.Y * manifold.LocalPoint.Y;
float pointBx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
transformB.R.Col2.X * tmp.Y;
float pointBy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
transformB.R.Col2.Y * tmp.Y;
normal.X = 1;
normal.Y = 0;
float result = (pointAx - pointBx) * (pointAx - pointBx) +
(pointAy - pointBy) * (pointAy - pointBy);
if (result > Settings.Epsilon * Settings.Epsilon)
{
float tmpNormalx = pointBx - pointAx;
float tmpNormaly = pointBy - pointAy;
float factor = 1f / (float)Math.Sqrt(tmpNormalx * tmpNormalx + tmpNormaly * tmpNormaly);
normal.X = tmpNormalx * factor;
normal.Y = tmpNormaly * factor;
}
Vector2 c = Vector2.Zero;
c.X = (pointAx + radiusA * normal.X) + (pointBx - radiusB * normal.X);
c.Y = (pointAy + radiusA * normal.Y) + (pointBy - radiusB * normal.Y);
points[0] = 0.5f * c;
}
break;
case ManifoldType.FaceA:
{
normal.X = transformA.R.Col1.X * manifold.LocalNormal.X +
transformA.R.Col2.X * manifold.LocalNormal.Y;
normal.Y = transformA.R.Col1.Y * manifold.LocalNormal.X +
transformA.R.Col2.Y * manifold.LocalNormal.Y;
float planePointx = transformA.Position.X + transformA.R.Col1.X * manifold.LocalPoint.X +
transformA.R.Col2.X * manifold.LocalPoint.Y;
float planePointy = transformA.Position.Y + transformA.R.Col1.Y * manifold.LocalPoint.X +
transformA.R.Col2.Y * manifold.LocalPoint.Y;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 tmp = manifold.Points[i].LocalPoint;
float clipPointx = transformB.Position.X + transformB.R.Col1.X * tmp.X +
transformB.R.Col2.X * tmp.Y;
float clipPointy = transformB.Position.Y + transformB.R.Col1.Y * tmp.X +
transformB.R.Col2.Y * tmp.Y;
float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y;
Vector2 c = Vector2.Zero;
c.X = (clipPointx + (radiusA - value) * normal.X) + (clipPointx - radiusB * normal.X);
c.Y = (clipPointy + (radiusA - value) * normal.Y) + (clipPointy - radiusB * normal.Y);
points[i] = 0.5f * c;
}
}
break;
case ManifoldType.FaceB:
{
normal.X = transformB.R.Col1.X * manifold.LocalNormal.X +
transformB.R.Col2.X * manifold.LocalNormal.Y;
normal.Y = transformB.R.Col1.Y * manifold.LocalNormal.X +
transformB.R.Col2.Y * manifold.LocalNormal.Y;
float planePointx = transformB.Position.X + transformB.R.Col1.X * manifold.LocalPoint.X +
transformB.R.Col2.X * manifold.LocalPoint.Y;
float planePointy = transformB.Position.Y + transformB.R.Col1.Y * manifold.LocalPoint.X +
transformB.R.Col2.Y * manifold.LocalPoint.Y;
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 tmp = manifold.Points[i].LocalPoint;
float clipPointx = transformA.Position.X + transformA.R.Col1.X * tmp.X +
transformA.R.Col2.X * tmp.Y;
float clipPointy = transformA.Position.Y + transformA.R.Col1.Y * tmp.X +
transformA.R.Col2.Y * tmp.Y;
float value = (clipPointx - planePointx) * normal.X + (clipPointy - planePointy) * normal.Y;
Vector2 c = Vector2.Zero;
c.X = (clipPointx - radiusA * normal.X) + (clipPointx + (radiusB - value) * normal.X);
c.Y = (clipPointy - radiusA * normal.Y) + (clipPointy + (radiusB - value) * normal.Y);
points[i] = 0.5f * c;
}
// Ensure normal points from A to B.
normal *= -1;
}
break;
default:
normal = Vector2.UnitY;
break;
}
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold"></param>
/// <param name="xfA"></param>
/// <param name="radiusA"></param>
/// <param name="xfB"></param>
/// <param name="radiusB"></param>
public WorldManifold(ref Manifold manifold,
ref Transform xfA, float radiusA,
ref Transform xfB, float radiusB)
{
_points = new FixedArray2<Vector2>();
if (manifold._pointCount == 0)
{
_normal = Vector2.UnitY;
return;
}
switch (manifold._type)
{
case ManifoldType.Circles:
{
Vector2 pointA = MathUtils.Multiply(ref xfA, manifold._localPoint);
Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
_normal = new Vector2(1.0f, 0.0f);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_epsilon * Settings.b2_epsilon)
{
_normal = pointB - pointA;
_normal.Normalize();
}
Vector2 cA = pointA + radiusA * _normal;
Vector2 cB = pointB - radiusB * _normal;
_points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
_normal = MathUtils.Multiply(ref xfA.R, manifold._localNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfB, manifold._points[i].LocalPoint);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
Vector2 cB = clipPoint - radiusB * _normal;
_points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
_normal = MathUtils.Multiply(ref xfB.R, manifold._localNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfA, manifold._points[i].LocalPoint);
Vector2 cA = clipPoint - radiusA * _normal;
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, _normal)) * _normal;
_points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
_normal *= -1;
}
break;
default:
_normal = Vector2.UnitY;
break;
}
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="xfA">The transform for A.</param>
/// <param name="radiusA">The radius for A.</param>
/// <param name="xfB">The transform for B.</param>
/// <param name="radiusB">The radius for B.</param>
/// <param name="normal">World vector pointing from A to B</param>
/// <param name="points">Torld contact point (point of intersection).</param>
public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 <Vector2> points)
{
normal = Vector2.Zero;
points = new FixedArray2 <Vector2>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new Vector2(1.0f, 0.0f);
Vector2 pointA = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
Vector2 pointB = Transform.Multiply(manifold.Points[0].LocalPoint, ref xfB);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
normal = Vector2.Normalize(pointB - pointA);
}
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
normal = Complex.Multiply(ref manifold.LocalNormal, ref xfA.q);
Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfA);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfB);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
normal = Complex.Multiply(ref manifold.LocalNormal, ref xfB.q);
Vector2 planePoint = Transform.Multiply(ref manifold.LocalPoint, ref xfB);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = Transform.Multiply(manifold.Points[i].LocalPoint, ref xfA);
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
/// <summary>
/// Clipping for contact manifolds.
/// </summary>
/// <param name="vOut">The v out.</param>
/// <param name="vIn">The v in.</param>
/// <param name="normal">The normal.</param>
/// <param name="offset">The offset.</param>
/// <param name="vertexIndexA">The vertex index A.</param>
/// <returns></returns>
private static int ClipSegmentToLine(out FixedArray2<ClipVertex> vOut, ref FixedArray2<ClipVertex> vIn,
Vector2 normal, float offset, int vertexIndexA)
{
vOut = new FixedArray2<ClipVertex>();
ClipVertex v0 = vIn[0];
ClipVertex v1 = vIn[1];
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = normal.X * v0.V.X + normal.Y * v0.V.Y - offset;
float distance1 = normal.X * v1.V.X + normal.Y * v1.V.Y - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = v0;
if (distance1 <= 0.0f) vOut[numOut++] = v1;
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
ClipVertex cv = vOut[numOut];
cv.V.X = v0.V.X + interp * (v1.V.X - v0.V.X);
cv.V.Y = v0.V.Y + interp * (v1.V.Y - v0.V.Y);
// VertexA is hitting edgeB.
cv.ID.Features.IndexA = (byte)vertexIndexA;
cv.ID.Features.IndexB = v0.ID.Features.IndexB;
cv.ID.Features.TypeA = (byte)ContactFeatureType.Vertex;
cv.ID.Features.TypeB = (byte)ContactFeatureType.Face;
vOut[numOut] = cv;
++numOut;
}
return numOut;
}
/// <summary>
/// Computes the world manifold data from this manifold with the specified properties for the two involved
/// objects.
/// </summary>
/// <param name="xfA">The transform of object A.</param>
/// <param name="radiusA">The radius of object A.</param>
/// <param name="xfB">The transform of object B.</param>
/// <param name="radiusB">The radius of object B.</param>
/// <param name="normal">The normal.</param>
/// <param name="points">The world contact points.</param>
public void ComputeWorldManifold(
WorldTransform xfA,
float radiusA,
WorldTransform xfB,
float radiusB,
out Vector2 normal,
out FixedArray2 <WorldPoint> points)
{
points = new FixedArray2 <WorldPoint>(); // satisfy out
switch (Type)
{
case ManifoldType.Circles:
{
normal = Vector2.UnitX;
var pointA = xfA.ToGlobal(LocalPoint);
var pointB = xfB.ToGlobal(Points[0].LocalPoint);
if (WorldPoint.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
// ReSharper disable RedundantCast Necessary for FarPhysics.
normal = (Vector2)(pointB - pointA);
// ReSharper restore RedundantCast
normal.Normalize();
}
var cA = pointA + radiusA * normal;
var cB = pointB - radiusB * normal;
points.Item1 = 0.5f * (cA + cB);
break;
}
case ManifoldType.FaceA:
{
normal = xfA.Rotation * LocalNormal;
var planePoint = xfA.ToGlobal(LocalPoint);
for (var i = 0; i < PointCount; ++i)
{
var clipPoint = xfB.ToGlobal(Points[i].LocalPoint);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var cA = clipPoint + (radiusA - Vector2Util.Dot((Vector2)(clipPoint - planePoint), normal)) * normal;
// ReSharper restore RedundantCast
var cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
break;
}
case ManifoldType.FaceB:
{
normal = xfB.Rotation * LocalNormal;
var planePoint = xfB.ToGlobal(LocalPoint);
for (var i = 0; i < PointCount; ++i)
{
var clipPoint = xfA.ToGlobal(Points[i].LocalPoint);
// ReSharper disable RedundantCast Necessary for FarPhysics.
var cB = clipPoint + (radiusB - Vector2Util.Dot((Vector2)(clipPoint - planePoint), normal)) * normal;
// ReSharper restore RedundantCast
var cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
break;
}
default:
throw new ArgumentOutOfRangeException();
}
}
public void UpdateCollisionPoints()
{
Collisions.Clear();
for (int i = 0; i < 4; i++)
CollidingSides[i] = false;
if (Body.ContactList != null)
{
Contact currentContact = Body.ContactList.Contact;
while (currentContact != null)
{ //tiene conto solo dei Contact relativi alla BodyFixture
//(non so perché il Body contiene dei Contact che non sono più relativi alla sua Fixture)
//e ignora quelli dovuti al giunto
if (((currentContact.FixtureA == BodyFixture) || (currentContact.FixtureB == BodyFixture)) &&
(currentContact.FixtureA != JointFixture) && (currentContact.FixtureB != JointFixture))
{
bool childJoint = false;
foreach (FenotipoCell child in ChildParts)
if ((currentContact.FixtureA == child.JointFixture) || (currentContact.FixtureB == child.JointFixture))
{
childJoint = true;
break;
}
//ignora la collisione anche se è dovuta al giunto di una parte figlia
if (!childJoint)
{
//estrae i punti di collisione del contatto corrente
FixedArray2<Vector2> points = new FixedArray2<Vector2>();
Vector2 vec = new Vector2();
currentContact.GetWorldManifold(out vec, out points);
for (int i = 0; i < 2; i++)
{
//alcuni Manifold contengono dei punti (0,0), non so perché. Vanno ignorati.
if (!points[i].Equals(Vector2.Zero))
{
bool belongsToThisPart = false;
ContactType contactType = ContactType.Corner;
Corner corner;
Side side = Side.Bottom;
if (IsCorner(points[i], out corner))
{
contactType = ContactType.Corner;
belongsToThisPart = true;
}
else if (IsSide(points[i], out side))
{
contactType = ContactType.Side;
belongsToThisPart = true;
}
//non so perché ma alcuni contatti, relativi alla fixture giusta, non stanno sul bordo della fixture stessa
//quindi prima verifico che siano o su un angolo o su un lato della parte
if (belongsToThisPart)
{
Collision newCollision = new Collision();
newCollision.Position = new Vector2(points[i].X, points[i].Y);
newCollision.OtherFixture = (currentContact.FixtureA == BodyFixture) ? currentContact.FixtureB : currentContact.FixtureA;
newCollision.Type = contactType;
if (contactType == ContactType.Corner)
newCollision.CollisionCorner = corner;
else
newCollision.CollisionSide = side;
Collisions.Add(newCollision);
}
}
}
}
}
//scorre la lista dei Contact
currentContact = currentContact.Next;
}
if (Collisions.Count > 0)
{
//una collisione è "risolta" se è già stata trattata
bool[] solvedCollisions = new bool[Collisions.Count];
/* cerco coppie di collisioni relative alla stessa fixture, il che accade quando due fixture sono appoggiate l'una sull'altra
* o una a fianco dell'altra e sono tra loro "parallele"
* In questo caso solo il lato interessato viene contrassegnato come interessato da una collisione.
* (l'algoritmo credo sia quadratico ma non penso sia un problema, spesso ci sono 2-3 collisioni, mai più di 5-6)
* */
for (int i = 0; i < Collisions.Count; i++)
for (int j = i + 1; j < Collisions.Count; j++)
{
if (Collisions[i].OtherFixture == Collisions[j].OtherFixture)
{
/*se entrambe le collisioni sono segnate su angoli facendo l'AND tra i due Corner e valutando
* la posizione dell'1 che si ottiene con RapidLog2 si ottiene il Side in comune tra i due
* Corner (es. TopRight = 0011, TopLeft = 1001, TopRight & TopLeft = 0001, Log2(0001) = 0 = Top)
*/
if((Collisions[i].Type == ContactType.Corner) && (Collisions[j].Type == ContactType.Corner))
{
int sideIndex = (int)Collisions[i].CollisionCorner & (int)Collisions[j].CollisionCorner;
if (sideIndex != 0)
{
sideIndex = Utils.RapidLog2(sideIndex);
CollidingSides[sideIndex] = true;
solvedCollisions[i] = true;
solvedCollisions[j] = true;
}
}
//se almeno una collisione è su un lato ho già il lato interessato da una delle collisioni
//registrate su un lato
else if (Collisions[i].Type == ContactType.Side)
{
CollidingSides[(int)Collisions[i].CollisionSide] = true;
solvedCollisions[i] = true;
solvedCollisions[j] = true;
}
else
{
CollidingSides[(int)Collisions[j].CollisionSide] = true;
solvedCollisions[i] = true;
solvedCollisions[j] = true;
}
}
}
/* restano solo le collisioni singole (cioè con Fixture con cui la BodyFixture attuale ha un solo
* punto di contatto): se è un Corner abilita entrambe i lati che fanno capo a quel vertice, altrimenti
* se è un Side abilita solo il relativo lato
*/
for (int i = 0; i < Collisions.Count; i++)
if (!solvedCollisions[i])
{
if (Collisions[i].Type == ContactType.Corner)
{
int sideIndex = Utils.RapidLog2((int)Collisions[i].CollisionCorner);
int otherSideIndex = Utils.RapidLog2((0xFFFE << sideIndex) & (int)Collisions[i].CollisionCorner);
CollidingSides[sideIndex] = true;
CollidingSides[otherSideIndex] = true;
}
else
CollidingSides[(int)Collisions[i].CollisionSide] = true;
}
}
}
}
public virtual void PreSolve(Contact contact, ref Manifold oldManifold)
{
/*Manifold manifold;
* contact.GetManifold(out manifold);
*
* if (manifold._pointCount == 0)
* {
* return;
* }
*
* Fixture fixtureA = contact.GetFixtureA();
* Fixture fixtureB = contact.GetFixtureB();
*
* FixedArray2<PointState> state1, state2;
* Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold);
*
* WorldManifold worldManifold;
* contact.GetWorldManifold(out worldManifold);
*
* for (int i = 0; i < manifold._pointCount && _pointCount < k_maxContactPoints; ++i)
* {
* if (fixtureA == null)
* {
* _points[i] = new ContactPoint();
* }
* ContactPoint cp = _points[_pointCount];
* cp.fixtureA = fixtureA;
* cp.fixtureB = fixtureB;
* cp.position = worldManifold._points[i];
* cp.normal = worldManifold._normal;
* cp.state = state2[i];
* _points[_pointCount] = cp;
++_pointCount;
* }*/
WorldManifold worldManifold;
contact.GetWorldManifold(out worldManifold);
FixedArray2 <PointState> state1 = new FixedArray2 <PointState>();
FixedArray2 <PointState> state2 = new FixedArray2 <PointState>();
Manifold manifold;
contact.GetManifold(out manifold);
Box2D.XNA.Collision.GetPointStates(out state1, out state2, ref oldManifold, ref manifold);
if (state2[0] == PointState.Add)
{
Body bodyA = contact.GetFixtureA().GetBody();
Body bodyB = contact.GetFixtureB().GetBody();
Vector2 point = worldManifold._points[0];
Vector2 vA = bodyA.GetLinearVelocityFromWorldPoint(point);
Vector2 vB = bodyB.GetLinearVelocityFromWorldPoint(point);
float approachVelocity = Vector2.Dot(Vector2.Subtract(vB, vA), worldManifold._normal);
if (approachVelocity < -0.1f)
{
sound.Play();
}
}
}
/// Clipping for contact manifolds.
public static int ClipSegmentToLine(out FixedArray2<ClipVertex> vOut, ref FixedArray2<ClipVertex> vIn,
Vector2 normal, float offset, int vertexIndexA)
{
vOut = new FixedArray2<ClipVertex>();
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vector2.Dot(normal, vIn[0].v) - offset;
float distance1 = Vector2.Dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
var cv = vOut[numOut];
cv.v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
// VertexA is hitting edgeB.
cv.id.Features.indexA = (byte)vertexIndexA;
cv.id.Features.indexB = vIn[0].id.Features.indexB;
cv.id.Features.typeA = (byte)ContactFeatureType.Vertex;
cv.id.Features.typeB = (byte)ContactFeatureType.Face;
vOut[numOut] = cv;
++numOut;
}
return numOut;
}
public static void GetPointStates(out FixedArray2<PointState> state1, out FixedArray2<PointState> state2,
ref Manifold manifold1, ref Manifold manifold2)
{
state1 = new FixedArray2<PointState>();
state2 = new FixedArray2<PointState>();
// Detect persists and removes.
for (int i = 0; i < manifold1._pointCount; ++i)
{
ContactID id = manifold1._points[i].Id;
state1[i] = PointState.Remove;
for (int j = 0; j < manifold2._pointCount; ++j)
{
if (manifold2._points[j].Id.Key == id.Key)
{
state1[i] = PointState.Persist;
break;
}
}
}
// Detect persists and adds.
for (int i = 0; i < manifold2._pointCount; ++i)
{
ContactID id = manifold2._points[i].Id;
state2[i] = PointState.Add;
for (int j = 0; j < manifold1._pointCount; ++j)
{
if (manifold1._points[j].Id.Key == id.Key)
{
state2[i] = PointState.Persist;
break;
}
}
}
}
public static void CollideEdgeAndPolygon(ref Manifold manifold,
EdgeShape edgeA, ref Transform xfA,
PolygonShape polygonB_in, ref Transform xfB)
{
manifold._pointCount = 0;
Transform xf;
MathUtils.MultiplyT(ref xfA, ref xfB, out xf);
// Create a polygon for edge shape A
s_polygonA.SetAsEdge(edgeA._vertex1, edgeA._vertex2);
// Build polygonB in frame A
s_polygonB._radius = polygonB_in._radius;
s_polygonB._vertexCount = polygonB_in._vertexCount;
s_polygonB._centroid = MathUtils.Multiply(ref xf, polygonB_in._centroid);
for (int i = 0; i < s_polygonB._vertexCount; ++i)
{
s_polygonB._vertices[i] = MathUtils.Multiply(ref xf, polygonB_in._vertices[i]);
s_polygonB._normals[i] = MathUtils.Multiply(ref xf.R, polygonB_in._normals[i]);
}
float totalRadius = s_polygonA._radius + s_polygonB._radius;
// Edge geometry
Vector2 v1 = edgeA._vertex1;
Vector2 v2 = edgeA._vertex2;
Vector2 e = v2 - v1;
Vector2 edgeNormal = new Vector2(e.Y, -e.X);
edgeNormal.Normalize();
// Determine side
bool isFrontSide = Vector2.Dot(edgeNormal, s_polygonB._centroid - v1) >= 0.0f;
if (isFrontSide == false)
{
edgeNormal = -edgeNormal;
}
// Compute primary separating axis
EPAxis edgeAxis = ComputeEdgeSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius);
if (edgeAxis.separation > totalRadius)
{
// Shapes are separated
return;
}
// Classify adjacent edges
FixedArray2<EdgeType> types = new FixedArray2<EdgeType>();
//types[0] = EdgeType.Isolated;
//types[1] = EdgeType.Isolated;
if (edgeA._hasVertex0)
{
Vector2 v0 = edgeA._vertex0;
float s = Vector2.Dot(edgeNormal, v0 - v1);
if (s > 0.1f * Settings.b2_linearSlop)
{
types[0] = EdgeType.Concave;
}
else if (s >= -0.1f * Settings.b2_linearSlop)
{
types[0] = EdgeType.Flat;
}
else
{
types[0] = EdgeType.Convex;
}
}
if (edgeA._hasVertex3)
{
Vector2 v3 = edgeA._vertex3;
float s = Vector2.Dot(edgeNormal, v3 - v2);
if (s > 0.1f * Settings.b2_linearSlop)
{
types[1] = EdgeType.Concave;
}
else if (s >= -0.1f * Settings.b2_linearSlop)
{
types[1] = EdgeType.Flat;
}
else
{
types[1] = EdgeType.Convex;
}
}
if (types[0] == EdgeType.Convex)
{
// Check separation on previous edge.
Vector2 v0 = edgeA._vertex0;
Vector2 e0 = v1 - v0;
Vector2 n0 = new Vector2(e0.Y, -e0.X);
n0.Normalize();
if (isFrontSide == false)
{
n0 = -n0;
}
EPAxis axis1 = ComputeEdgeSeperation(v0, v1, n0, s_polygonB, totalRadius);
if (axis1.separation > edgeAxis.separation)
{
// The polygon should collide with previous edge
return;
}
}
if (types[1] == EdgeType.Convex)
{
// Check separation on next edge.
Vector2 v3 = edgeA._vertex3;
Vector2 e2 = v3 - v2;
Vector2 n2 = new Vector2(e2.Y, -e2.X);
n2.Normalize();
if (isFrontSide == false)
{
n2 = -n2;
}
EPAxis axis2 = ComputeEdgeSeperation(v2, v3, n2, s_polygonB, totalRadius);
if (axis2.separation > edgeAxis.separation)
{
// The polygon should collide with the next edge
return;
}
}
EPAxis polygonAxis = ComputePolygonSeperation(v1, v2, edgeNormal, s_polygonB, totalRadius);
if (polygonAxis.separation > totalRadius)
{
return;
}
// Use hysteresis for jitter reduction.
float k_relativeTol = 0.98f;
float k_absoluteTol = 0.001f;
EPAxis primaryAxis;
if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
PolygonShape poly1;
PolygonShape poly2;
if (primaryAxis.type == EPAxisType.EdgeA)
{
poly1 = s_polygonA;
poly2 = s_polygonB;
if (isFrontSide == false)
{
primaryAxis.index = 1;
}
manifold._type = ManifoldType.FaceA;
}
else
{
poly1 = s_polygonB;
poly2 = s_polygonA;
manifold._type = ManifoldType.FaceB;
}
int edge1 = primaryAxis.index;
FixedArray2<ClipVertex> incidentEdge = new FixedArray2<ClipVertex>();
FindIncidentEdge(ref incidentEdge, poly1, primaryAxis.index, poly2);
int count1 = poly1._vertexCount;
int iv1 = edge1;
int iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
Vector2 v11 = poly1._vertices[iv1];
Vector2 v12 = poly1._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) + totalRadius;
float sideOffset2 = Vector2.Dot(tangent, v12) + totalRadius;
// 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.b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, ref clipPoints1, tangent, sideOffset2, iv2);
if (np < Settings.b2_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, normal);
manifold._localPoint = MathUtils.MultiplyT(ref xf, planePoint);
}
int pointCount = 0;
for (int i = 0; i < Settings.b2_maxManifoldPoints; ++i)
{
float separation;
separation = Vector2.Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
ManifoldPoint cp = manifold._points[pointCount];
if (primaryAxis.type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MultiplyT(ref xf, clipPoints2[i].v);
cp.Id = clipPoints2[i].id;
}
else
{
cp.LocalPoint = clipPoints2[i].v;
cp.Id.Features.typeA = clipPoints2[i].id.Features.typeB;
cp.Id.Features.typeB = clipPoints2[i].id.Features.typeA;
cp.Id.Features.indexA = clipPoints2[i].id.Features.indexB;
cp.Id.Features.indexB = clipPoints2[i].id.Features.indexA;
}
manifold._points[pointCount] = cp;
if (cp.Id.Features.typeA == (byte)ContactFeatureType.Vertex && types[cp.Id.Features.indexA] == EdgeType.Flat)
{
continue;
}
++pointCount;
}
}
manifold._pointCount = pointCount;
}
// This function collides and edge and a polygon.
// This takes into account edge adjacency.
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
public static bool CollideEdgeAndPolygon(
Fixture fixtureA,
WorldTransform xfA,
Fixture fixtureB,
WorldTransform xfB,
out Manifold manifold)
{
manifold = new Manifold();
var edgeA = fixtureA as EdgeFixture;
var polygonB = fixtureB as PolygonFixture;
System.Diagnostics.Debug.Assert(edgeA != null);
System.Diagnostics.Debug.Assert(polygonB != null);
// This holds polygon B expressed in frame A.
var tpv = new Vector2[Settings.MaxPolygonVertices];
var tpn = new Vector2[Settings.MaxPolygonVertices];
Vector2 normal0 = Vector2.Zero, normal1, normal2 = Vector2.Zero;
var xf = xfA.MulT(xfB);
var centroidB = xf.ToOther(polygonB.Centroid);
var v0 = edgeA.Vertex0;
var v1 = edgeA.Vertex1;
var v2 = edgeA.Vertex2;
var v3 = edgeA.Vertex3;
var hasVertex0 = edgeA.HasVertex0;
var hasVertex3 = edgeA.HasVertex3;
var edge1 = v2 - v1;
edge1.Normalize();
normal1.X = edge1.Y;
normal1.Y = -edge1.X;
var offset1 = Vector2Util.Dot(normal1, centroidB - v1);
var offset0 = 0.0f;
var offset2 = 0.0f;
var convex1 = false;
var convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
var edge0 = v1 - v0;
edge0.Normalize();
normal0.X = edge0.Y;
normal0.Y = -edge0.X;
convex1 = Vector2Util.Cross(ref edge0, ref edge1) >= 0.0f;
offset0 = Vector2Util.Dot(normal0, centroidB - v0);
}
// Is there a following edge?
if (hasVertex3)
{
var edge2 = v3 - v2;
edge2.Normalize();
normal2.X = edge2.Y;
normal2.Y = -edge2.X;
convex2 = Vector2Util.Cross(ref edge1, ref edge2) > 0.0f;
offset2 = Vector2Util.Dot(normal2, centroidB - v2);
}
// Determine front or back collision. Determine collision normal limits.
bool front;
Vector2 normal, lowerLimit, upperLimit;
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
}
else if (convex1)
{
front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal1;
}
}
else if (convex2)
{
front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal0;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
}
else
{
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = normal1;
}
}
}
else
{
front = offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
}
// Get polygonB in frameA.
var tpc = polygonB.Count;
for (var i = 0; i < tpc; ++i)
{
tpv[i] = xf.ToOther(polygonB.Vertices[i]);
tpn[i] = xf.Rotation * polygonB.Normals[i];
}
const float radius = 2.0f * Settings.PolygonRadius;
Axis edgeAxis;
edgeAxis.Type = Axis.AxisType.EdgeA;
edgeAxis.Index = front ? 0 : 1;
edgeAxis.Separation = float.MaxValue;
for (var i = 0; i < tpc; ++i)
{
var s = Vector2Util.Dot(normal, tpv[i] - v1);
if (s < edgeAxis.Separation)
{
edgeAxis.Separation = s;
}
}
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == Axis.AxisType.None)
{
return(false);
}
if (edgeAxis.Separation > radius)
{
return(false);
}
Axis polygonAxis;
polygonAxis.Type = Axis.AxisType.None;
polygonAxis.Index = -1;
polygonAxis.Separation = float.MinValue;
Vector2 perp;
perp.X = -normal.Y;
perp.Y = normal.X;
for (var i = 0; i < tpc; ++i)
{
var n = -tpn[i];
var s1 = Vector2Util.Dot(n, tpv[i] - v1);
var s2 = Vector2Util.Dot(n, tpv[i] - v2);
var s = System.Math.Min(s1, s2);
if (s > radius)
{
// No collision
polygonAxis.Type = Axis.AxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
break;
}
// Adjacency
if (Vector2Util.Dot(ref n, ref perp) >= 0.0f)
{
if (Vector2Util.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
else
{
if (Vector2Util.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
if (s > polygonAxis.Separation)
{
polygonAxis.Type = Axis.AxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
}
}
if (polygonAxis.Type != Axis.AxisType.None && polygonAxis.Separation > radius)
{
return(false);
}
// Use hysteresis for jitter reduction.
const float relativeTol = 0.98f;
const float absoluteTol = 0.001f;
Axis primaryAxis;
if (polygonAxis.Type == Axis.AxisType.None)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.Separation > relativeTol * edgeAxis.Separation + absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
FixedArray2 <ClipVertex> incidentEdge;
// Reference face used for clipping
int rfi1, rfi2;
Vector2 rfv1, rfv2;
Vector2 rfnormal;
Vector2 rfsideNormal1;
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
manifold.Type = Manifold.ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
var bestIndex = 0;
var bestValue = Vector2Util.Dot(ref normal, ref tpn[0]);
for (var i = 1; i < tpc; ++i)
{
var value = Vector2Util.Dot(ref normal, ref tpn[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
var i1 = bestIndex;
var i2 = i1 + 1 < tpc ? i1 + 1 : 0;
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
Vertex = tpv[i1],
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)i1,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
},
Item2 = new ClipVertex
{
Vertex = tpv[i2],
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)i2,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
}
};
if (front)
{
rfi1 = 0;
rfi2 = 1;
rfv1 = v1;
rfv2 = v2;
rfnormal = normal1;
}
else
{
rfi1 = 1;
rfi2 = 0;
rfv1 = v2;
rfv2 = v1;
rfnormal = -normal1;
}
}
else
{
manifold.Type = Manifold.ManifoldType.FaceB;
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
Vertex = v1,
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)primaryAxis.Index,
TypeA = (byte)ContactFeature.FeatureType.Vertex,
TypeB = (byte)ContactFeature.FeatureType.Face
}
}
},
Item2 = new ClipVertex
{
Vertex = v2,
Id =
{
Feature =
{
IndexA = 0,
IndexB = (byte)primaryAxis.Index,
TypeA = (byte)ContactFeature.FeatureType.Vertex,
TypeB = (byte)ContactFeature.FeatureType.Face
}
}
}
};
rfi1 = primaryAxis.Index;
rfi2 = rfi1 + 1 < tpc ? rfi1 + 1 : 0;
rfv1 = tpv[rfi1];
rfv2 = tpv[rfi2];
rfnormal = tpn[rfi1];
}
rfsideNormal1.X = rfnormal.Y;
rfsideNormal1.Y = -rfnormal.X;
var rfsideNormal2 = -rfsideNormal1;
var rfsideOffset1 = Vector2Util.Dot(ref rfsideNormal1, ref rfv1);
var rfsideOffset2 = Vector2Util.Dot(ref rfsideNormal2, ref rfv2);
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1, clipPoints2;
// Clip to box side 1
var np = ClipSegmentToLine(
out clipPoints1,
incidentEdge,
rfsideNormal1,
rfsideOffset1,
rfi1);
if (np < 2)
{
return(false);
}
// Clip to negative box side 1
np = ClipSegmentToLine(
out clipPoints2,
clipPoints1,
rfsideNormal2,
rfsideOffset2,
rfi2);
if (np < 2)
{
return(false);
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
manifold.LocalPoint = rfv1;
manifold.LocalNormal = rfnormal;
}
else
{
manifold.LocalPoint = polygonB.Vertices[rfi1];
manifold.LocalNormal = polygonB.Normals[rfi1];
}
var pointCount = 0;
for (var i = 0; i < 2; ++i)
{
if (Vector2Util.Dot(rfnormal, clipPoints2[i].Vertex - rfv1) <= radius)
{
var cp = manifold.Points[pointCount];
if (primaryAxis.Type == Axis.AxisType.EdgeA)
{
cp.LocalPoint = xf.FromOther(clipPoints2[i].Vertex);
cp.Id = clipPoints2[i].Id;
}
else
{
cp.LocalPoint = clipPoints2[i].Vertex;
cp.Id.Feature.TypeA = clipPoints2[i].Id.Feature.TypeB;
cp.Id.Feature.TypeB = clipPoints2[i].Id.Feature.TypeA;
cp.Id.Feature.IndexA = clipPoints2[i].Id.Feature.IndexB;
cp.Id.Feature.IndexB = clipPoints2[i].Id.Feature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
return(pointCount > 0);
}
private static void FindIncidentEdge(out FixedArray2<ClipVertex> c, PolygonShape poly1, ref Transform xf1, int edge1, PolygonShape poly2, ref Transform xf2)
{
c = new FixedArray2<ClipVertex>();
Vertices normals1 = poly1.Normals;
int count2 = poly2.Vertices.Count;
Vertices vertices2 = poly2.Vertices;
Vertices normals2 = poly2.Normals;
Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = MathUtils.MulT(xf2.q, MathUtils.Mul(xf1.q, normals1[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.MaxFloat;
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;
ClipVertex cv0 = c[0];
cv0.V = MathUtils.Mul(ref xf2, vertices2[i1]);
cv0.ID.Features.IndexA = (byte)edge1;
cv0.ID.Features.IndexB = (byte)i1;
cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
c[0] = cv0;
ClipVertex cv1 = c[1];
cv1.V = MathUtils.Mul(ref xf2, vertices2[i2]);
cv1.ID.Features.IndexA = (byte)edge1;
cv1.ID.Features.IndexB = (byte)i2;
cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
c[1] = cv1;
}
// 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 bool CollidePolygons(
Fixture fixtureA,
WorldTransform transformA,
Fixture fixtureB,
WorldTransform transformB,
out Manifold manifold)
{
manifold = new Manifold();
var polygonA = fixtureA as PolygonFixture;
var polygonB = fixtureB as PolygonFixture;
System.Diagnostics.Debug.Assert(polygonA != null);
System.Diagnostics.Debug.Assert(polygonB != null);
var totalRadius = polygonA.Radius + polygonB.Radius;
int edgeA;
var separationA = FindMaxSeparation(out edgeA, polygonA, transformA, polygonB, transformB);
if (separationA > totalRadius)
{
return(false);
}
int edgeB;
var separationB = FindMaxSeparation(out edgeB, polygonB, transformB, polygonA, transformA);
if (separationB > totalRadius)
{
return(false);
}
PolygonFixture polygon1; // reference polygon
PolygonFixture polygon2; // incident polygon
WorldTransform transform1, transform2;
int edge1; // reference edge
bool flip;
const float relativeTol = 0.98f;
const float absoluteTol = 0.001f;
if (separationB > relativeTol * separationA + absoluteTol)
{
polygon1 = polygonB;
polygon2 = polygonA;
transform1 = transformB;
transform2 = transformA;
edge1 = edgeB;
manifold.Type = Manifold.ManifoldType.FaceB;
flip = true;
}
else
{
polygon1 = polygonA;
polygon2 = polygonB;
transform1 = transformA;
transform2 = transformB;
edge1 = edgeA;
manifold.Type = Manifold.ManifoldType.FaceA;
flip = false;
}
// Transformation mapping from the second polygon's frame of reference to
// first one's. We use this to directly map points around, without getting
// into the global coordinate system.
var transform21 = transform1.MulT(transform2);
// Begin inlined FindIncidentEdge()
FixedArray2 <ClipVertex> incidentEdge;
{
System.Diagnostics.Debug.Assert(0 <= edge1 && edge1 < polygon1.Count);
var normals1 = polygon1.Normals;
var vertices2 = polygon2.Vertices;
var normals2 = polygon2.Normals;
var count2 = polygon2.Count;
// Get the normal of the reference edge in poly2's frame.
var normal12 = -transform2.Rotation * (transform1.Rotation * normals1[edge1]);
// Find the incident edge on poly2 by finding the clip vertices
// for the incident edge.
// Get the face whose own normal has the smallest angular
// difference to the incident normal.
var edge2 = 0;
var minDot = float.MaxValue;
for (var i = 0; i < count2; ++i)
{
var dot = Vector2Util.Dot(ref normal12, ref normals2[i]);
if (dot < minDot)
{
minDot = dot;
edge2 = i;
}
}
// The edge's index coincides with the first vertex used to define
// that edge, so we can use that and wrap around as necessary for the
// second one.
var index21 = edge2;
var index22 = index21 + 1 < count2 ? index21 + 1 : 0;
// Get the incident edge as defined by its two vertices, in the first
// polygon's frame of reference.
incidentEdge = new FixedArray2 <ClipVertex>
{
Item1 = new ClipVertex
{
//Vertex = xf2.ToGlobal(vertices2[i1]),
Vertex = transform21.ToOther(vertices2[index21]),
Id =
{
Feature =
{
IndexA = (byte)edge1,
IndexB = (byte)index21,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
},
Item2 = new ClipVertex
{
//Vertex = xf2.ToGlobal(vertices2[i2]),
Vertex = transform21.ToOther(vertices2[index22]),
Id =
{
Feature =
{
IndexA = (byte)edge1,
IndexB = (byte)index22,
TypeA = (byte)ContactFeature.FeatureType.Face,
TypeB = (byte)ContactFeature.FeatureType.Vertex
}
}
}
};
}
// End inlined FindIncidentEdge()
var vertices1 = polygon1.Vertices;
var count1 = polygon1.Count;
var index11 = edge1;
var index12 = edge1 + 1 < count1 ? edge1 + 1 : 0;
var vertex11 = vertices1[index11];
var vertex12 = vertices1[index12];
var tangent1 = vertex12 - vertex11;
tangent1.Normalize();
var normal1 = Vector2Util.Cross(ref tangent1, 1);
var planePoint1 = 0.5f * (vertex11 + vertex12);
// Face offset.
var frontOffset = Vector2Util.Dot(ref normal1, ref vertex11);
// Side offsets, extended by polytope skin thickness.
var sideOffset1 = -Vector2Util.Dot(ref tangent1, ref vertex11) + totalRadius;
var sideOffset2 = Vector2Util.Dot(ref tangent1, ref vertex12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1, clipPoints2;
// Clip to box side 1
var np = ClipSegmentToLine(out clipPoints1, incidentEdge, -tangent1, sideOffset1, index11);
if (np < 2)
{
return(false);
}
// Clip to negative box side 1
np = ClipSegmentToLine(out clipPoints2, clipPoints1, tangent1, sideOffset2, index12);
if (np < 2)
{
return(false);
}
// Now clipPoints2 contains the clipped points.
manifold.LocalNormal = normal1;
manifold.LocalPoint = planePoint1;
var pointCount = 0;
for (var i = 0; i < 2; ++i)
{
//if (Vector2Util.Dot(normal1g, clipPoints2[i].Vertex) - frontOffset <= totalRadius)
if (Vector2Util.Dot(normal1, clipPoints2[i].Vertex) - frontOffset <= totalRadius)
{
var cp = manifold.Points[pointCount];
//cp.localPoint = transform2.ToLocal(clipPoints2[i].Vertex);
cp.LocalPoint = transform21.FromOther(clipPoints2[i].Vertex);
cp.Id = clipPoints2[i].Id;
if (flip)
{
// Swap features
var cf = cp.Id.Feature;
cp.Id.Feature.IndexA = cf.IndexB;
cp.Id.Feature.IndexB = cf.IndexA;
cp.Id.Feature.TypeA = cf.TypeB;
cp.Id.Feature.TypeB = cf.TypeA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
return(pointCount > 0);
}
void PreparePipelines(Pipeline pipeline, GraphicsDevice device, RenderPass renderPass, Material material)
{
VkPipelineInputAssemblyStateCreateInfo inputAssemblyState =
Initializers.pipelineInputAssemblyStateCreateInfo(
VkPrimitiveTopology.TriangleList,
0,
False);
VkPipelineRasterizationStateCreateInfo rasterizationState =
Initializers.pipelineRasterizationStateCreateInfo(
VkPolygonMode.Fill,
VkCullModeFlags.Back,
VkFrontFace.Clockwise,
0);
if (material.wireframe && device.DeviceFeatures.fillModeNonSolid == 1)
{
rasterizationState.cullMode = VkCullModeFlags.None;
rasterizationState.polygonMode = VkPolygonMode.Line;
rasterizationState.lineWidth = 1.0f;
}
VkPipelineColorBlendAttachmentState blendAttachmentState =
Initializers.pipelineColorBlendAttachmentState(
0xf,
False);
VkPipelineColorBlendStateCreateInfo colorBlendState =
Initializers.pipelineColorBlendStateCreateInfo(
1,
&blendAttachmentState);
VkPipelineDepthStencilStateCreateInfo depthStencilState =
Initializers.pipelineDepthStencilStateCreateInfo(
True,
True,
VkCompareOp.LessOrEqual);
VkPipelineViewportStateCreateInfo viewportState =
Initializers.pipelineViewportStateCreateInfo(1, 1, 0);
VkPipelineMultisampleStateCreateInfo multisampleState =
Initializers.pipelineMultisampleStateCreateInfo(
VkSampleCountFlags.Count1,
0);
FixedArray2 <VkDynamicState> dynamicStateEnables = new FixedArray2 <VkDynamicState>(
VkDynamicState.Viewport,
VkDynamicState.Scissor);
VkPipelineDynamicStateCreateInfo dynamicState =
Initializers.pipelineDynamicStateCreateInfo(
&dynamicStateEnables.First,
dynamicStateEnables.Count,
0);
// Solid rendering pipeline
// Load shaders
FixedArray2 <VkPipelineShaderStageCreateInfo> shaderStages = new FixedArray2 <VkPipelineShaderStageCreateInfo>(
material.shaderPair.GetVertPipeline(),
material.shaderPair.GetFragPipeline());
VkGraphicsPipelineCreateInfo pipelineCreateInfo =
Initializers.pipelineCreateInfo(
pipeline.pipelineLayout,
renderPass.vkRenderPass,
0);
var via = vertices_inputState;
pipelineCreateInfo.pVertexInputState = &via;
pipelineCreateInfo.pInputAssemblyState = &inputAssemblyState;
pipelineCreateInfo.pRasterizationState = &rasterizationState;
pipelineCreateInfo.pColorBlendState = &colorBlendState;
pipelineCreateInfo.pMultisampleState = &multisampleState;
pipelineCreateInfo.pViewportState = &viewportState;
pipelineCreateInfo.pDepthStencilState = &depthStencilState;
pipelineCreateInfo.pDynamicState = &dynamicState;
pipelineCreateInfo.stageCount = shaderStages.Count;
pipelineCreateInfo.pStages = &shaderStages.First;
Util.CheckResult(vkCreateGraphicsPipelines(device.device, device.pipelineCache.vkPipelineCache, 1, &pipelineCreateInfo, null, out pipeline.vkPipeline));
}
public void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
_xf = MathUtils.MulT(xfA, xfB);
_centroidB = MathUtils.Mul(ref _xf, polygonB.MassData.Centroid);
_v0 = edgeA.Vertex0;
_v1 = edgeA._vertex1;
_v2 = edgeA._vertex2;
_v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
Vector2 edge1 = _v2 - _v1;
edge1.Normalize();
_normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(_normal1, _centroidB - _v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vector2 edge0 = _v1 - _v0;
edge0.Normalize();
_normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(_normal0, _centroidB - _v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vector2 edge2 = _v3 - _v2;
edge2.Normalize();
_normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(_normal2, _centroidB - _v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
}
else if (convex1)
{
_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal1;
}
}
else if (convex2)
{
_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal0;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal0;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
}
else
{
_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = -_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal2;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
}
else
{
_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = _normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = -_normal2;
_upperLimit = _normal1;
}
}
}
else
{
_front = offset1 >= 0.0f;
if (_front)
{
_normal = _normal1;
_lowerLimit = -_normal1;
_upperLimit = -_normal1;
}
else
{
_normal = -_normal1;
_lowerLimit = _normal1;
_upperLimit = _normal1;
}
}
// Get polygonB in frameA
_polygonB.Count = polygonB.Vertices.Count;
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
_polygonB.Vertices[i] = MathUtils.Mul(ref _xf, polygonB.Vertices[i]);
_polygonB.Normals[i] = MathUtils.Mul(_xf.q, polygonB.Normals[i]);
}
_radius = 2.0f * Settings.PolygonRadius;
manifold.PointCount = 0;
EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
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;
}
FixedArray2 <ClipVertex> ie = new FixedArray2 <ClipVertex>();
ReferenceFace rf;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vector2.Dot(_normal, _polygonB.Normals[0]);
for (int i = 1; i < _polygonB.Count; ++i)
{
float value = Vector2.Dot(_normal, _polygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < _polygonB.Count ? i1 + 1 : 0;
ie.Value0.V = _polygonB.Vertices[i1];
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)i1;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
ie.Value1.V = _polygonB.Vertices[i2];
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)i2;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
if (_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = _v1;
rf.v2 = _v2;
rf.Normal = _normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = _v2;
rf.v2 = _v1;
rf.Normal = -_normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ie.Value0.V = _v1;
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Face;
ie.Value1.V = _v2;
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Face;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < _polygonB.Count ? rf.i1 + 1 : 0;
rf.v1 = _polygonB.Vertices[rf.i1];
rf.v2 = _polygonB.Vertices[rf.i2];
rf.Normal = _polygonB.Normals[rf.i1];
}
rf.SideNormal1 = new Vector2(rf.Normal.Y, -rf.Normal.X);
rf.SideNormal2 = -rf.SideNormal1;
rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1);
rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
FixedArray2 <ClipVertex> clipPoints1;
FixedArray2 <ClipVertex> clipPoints2;
int np;
// Clip to box side 1
np = Collision.ClipSegmentToLine(out clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = rf.Normal;
manifold.LocalPoint = rf.v1;
}
else
{
manifold.LocalNormal = polygonB.Normals[rf.i1];
manifold.LocalPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(rf.Normal, clipPoints2[i].V - rf.v1);
if (separation <= _radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref _xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.ContactFeature.TypeA = clipPoints2[i].ID.ContactFeature.TypeB;
cp.Id.ContactFeature.TypeB = clipPoints2[i].ID.ContactFeature.TypeA;
cp.Id.ContactFeature.IndexA = clipPoints2[i].ID.ContactFeature.IndexB;
cp.Id.ContactFeature.IndexB = clipPoints2[i].ID.ContactFeature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
/// <param name="manifold">The manifold.</param>
/// <param name="xfA">The transform for A.</param>
/// <param name="radiusA">The radius for A.</param>
/// <param name="xfB">The transform for B.</param>
/// <param name="radiusB">The radius for B.</param>
/// <param name="normal">World vector pointing from A to B</param>
/// <param name="points">Torld contact point (point of intersection).</param>
public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB,
float radiusB, out System.Numerics.Vector2 normal, out FixedArray2 <System.Numerics.Vector2> points)
{
normal = System.Numerics.Vector2.Zero;
points = new FixedArray2 <System.Numerics.Vector2>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new System.Numerics.Vector2(1.0f, 0.0f);
var pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
var pointB = MathUtils.Mul(ref xfB, manifold.Points[0].LocalPoint);
if (System.Numerics.Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
normal = pointB - pointA;
Nez.Vector2Ext.Normalize(ref normal);
}
var cA = pointA + radiusA * normal;
var cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
break;
}
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.Q, manifold.LocalNormal);
var planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
var clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
var cA = clipPoint + (radiusA - System.Numerics.Vector2.Dot(clipPoint - planePoint, normal)) * normal;
var cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
}
break;
}
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.Q, manifold.LocalNormal);
var planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
var clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
var cB = clipPoint + (radiusB - System.Numerics.Vector2.Dot(clipPoint - planePoint, normal)) * normal;
var cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
}
// Ensure normal points from A to B.
normal = -normal;
break;
}
}
}
public static void Collide(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
// Algorithm:
// 1. Classify v1 and v2
// 2. Classify polygon centroid as front or back
// 3. Flip normal if necessary
// 4. Initialize normal range to [-pi, pi] about face normal
// 5. Adjust normal range according to adjacent edges
// 6. Visit each separating axes, only accept axes within the range
// 7. Return if _any_ axis indicates separation
// 8. Clip
bool front;
Vector2 lowerLimit, upperLimit;
Vector2 normal;
Vector2 normal0 = Vector2.Zero;
Vector2 normal2 = Vector2.Zero;
Transform xf = MathUtils.MulT(xfA, xfB);
Vector2 centroidB = MathUtils.Mul(ref xf, polygonB.MassData.Centroid);
Vector2 v0 = edgeA.Vertex0;
Vector2 v1 = edgeA._vertex1;
Vector2 v2 = edgeA._vertex2;
Vector2 v3 = edgeA.Vertex3;
bool hasVertex0 = edgeA.HasVertex0;
bool hasVertex3 = edgeA.HasVertex3;
Vector2 edge1 = v2 - v1;
edge1.Normalize();
Vector2 normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = Vector2.Dot(normal1, centroidB - v1);
float offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
Vector2 edge0 = v1 - v0;
edge0.Normalize();
normal0 = new Vector2(edge0.Y, -edge0.X);
convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
offset0 = Vector2.Dot(normal0, centroidB - v0);
}
// Is there a following edge?
if (hasVertex3)
{
Vector2 edge2 = v3 - v2;
edge2.Normalize();
normal2 = new Vector2(edge2.Y, -edge2.X);
convex2 = MathUtils.Cross(edge1, edge2) > 0.0f;
offset2 = Vector2.Dot(normal2, centroidB - v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
}
else if (convex1)
{
front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal1;
}
}
else if (convex2)
{
front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = -normal0;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = -normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
front = offset0 >= 0.0f || offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal0;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
}
else
{
front = offset0 >= 0.0f && offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = -normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
front = offset1 >= 0.0f || offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal2;
}
else
{
normal = -normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
}
else
{
front = offset1 >= 0.0f && offset2 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = normal1;
}
else
{
normal = -normal1;
lowerLimit = -normal2;
upperLimit = normal1;
}
}
}
else
{
front = offset1 >= 0.0f;
if (front)
{
normal = normal1;
lowerLimit = -normal1;
upperLimit = -normal1;
}
else
{
normal = -normal1;
lowerLimit = normal1;
upperLimit = normal1;
}
}
// Get polygonB in frameA
Vector2[] normals = new Vector2[Settings.MaxPolygonVertices];
Vector2[] vertices = new Vector2[Settings.MaxPolygonVertices];
int count = polygonB.Vertices.Count;
for (int i = 0; i < polygonB.Vertices.Count; ++i)
{
vertices[i] = MathUtils.Mul(ref xf, polygonB.Vertices[i]);
normals[i] = MathUtils.Mul(xf.q, polygonB.Normals[i]);
}
float radius = polygonB.Radius + edgeA.Radius;
manifold.PointCount = 0;
//Velcro: ComputeEdgeSeparation() was manually inlined here
EPAxis edgeAxis;
edgeAxis.Type = EPAxisType.EdgeA;
edgeAxis.Index = front ? 0 : 1;
edgeAxis.Separation = MathConstants.MaxFloat;
for (int i = 0; i < count; ++i)
{
float s = Vector2.Dot(normal, vertices[i] - v1);
if (s < edgeAxis.Separation)
{
edgeAxis.Separation = s;
}
}
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.Type == EPAxisType.Unknown)
{
return;
}
if (edgeAxis.Separation > radius)
{
return;
}
//Velcro: ComputePolygonSeparation() was manually inlined here
EPAxis polygonAxis;
polygonAxis.Type = EPAxisType.Unknown;
polygonAxis.Index = -1;
polygonAxis.Separation = -MathConstants.MaxFloat;
Vector2 perp = new Vector2(-normal.Y, normal.X);
for (int i = 0; i < count; ++i)
{
Vector2 n = -normals[i];
float s1 = Vector2.Dot(n, vertices[i] - v1);
float s2 = Vector2.Dot(n, vertices[i] - v2);
float s = Math.Min(s1, s2);
if (s > radius)
{
// No collision
polygonAxis.Type = EPAxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
break;
}
// Adjacency
if (Vector2.Dot(n, perp) >= 0.0f)
{
if (Vector2.Dot(n - upperLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
else
{
if (Vector2.Dot(n - lowerLimit, normal) < -Settings.AngularSlop)
{
continue;
}
}
if (s > polygonAxis.Separation)
{
polygonAxis.Type = EPAxisType.EdgeB;
polygonAxis.Index = i;
polygonAxis.Separation = s;
}
}
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;
}
FixedArray2 <ClipVertex> ie = new FixedArray2 <ClipVertex>();
ReferenceFace rf;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = Vector2.Dot(normal, normals[0]);
for (int i = 1; i < count; ++i)
{
float value = Vector2.Dot(normal, normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < count ? i1 + 1 : 0;
ie.Value0.V = vertices[i1];
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)i1;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
ie.Value1.V = vertices[i2];
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)i2;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Face;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Vertex;
if (front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = v1;
rf.v2 = v2;
rf.Normal = normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = v2;
rf.v2 = v1;
rf.Normal = -normal1;
}
}
else
{
manifold.Type = ManifoldType.FaceB;
ie.Value0.V = v1;
ie.Value0.ID.ContactFeature.IndexA = 0;
ie.Value0.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value0.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value0.ID.ContactFeature.TypeB = ContactFeatureType.Face;
ie.Value1.V = v2;
ie.Value1.ID.ContactFeature.IndexA = 0;
ie.Value1.ID.ContactFeature.IndexB = (byte)primaryAxis.Index;
ie.Value1.ID.ContactFeature.TypeA = ContactFeatureType.Vertex;
ie.Value1.ID.ContactFeature.TypeB = ContactFeatureType.Face;
rf.i1 = primaryAxis.Index;
rf.i2 = rf.i1 + 1 < count ? rf.i1 + 1 : 0;
rf.v1 = vertices[rf.i1];
rf.v2 = vertices[rf.i2];
rf.Normal = normals[rf.i1];
}
rf.SideNormal1 = new Vector2(rf.Normal.Y, -rf.Normal.X);
rf.SideNormal2 = -rf.SideNormal1;
rf.SideOffset1 = Vector2.Dot(rf.SideNormal1, rf.v1);
rf.SideOffset2 = Vector2.Dot(rf.SideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
// Clip to box side 1
int np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints1, ref ie, rf.SideNormal1, rf.SideOffset1, rf.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = Collision.ClipSegmentToLine(out FixedArray2 <ClipVertex> clipPoints2, ref clipPoints1, rf.SideNormal2, rf.SideOffset2, rf.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = rf.Normal;
manifold.LocalPoint = rf.v1;
}
else
{
manifold.LocalNormal = polygonB.Normals[rf.i1];
manifold.LocalPoint = polygonB.Vertices[rf.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = Vector2.Dot(rf.Normal, clipPoints2[i].V - rf.v1);
if (separation <= radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(ref xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].ID;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.ContactFeature.TypeA = clipPoints2[i].ID.ContactFeature.TypeB;
cp.Id.ContactFeature.TypeB = clipPoints2[i].ID.ContactFeature.TypeA;
cp.Id.ContactFeature.IndexA = clipPoints2[i].ID.ContactFeature.IndexB;
cp.Id.ContactFeature.IndexB = clipPoints2[i].ID.ContactFeature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
private static void FindIncidentEdge(ref FixedArray2<ClipVertex> c, EPProxy proxy1, int edge1, EPProxy proxy2)
{
int count2 = proxy2.Count;
Debug.Assert(0 <= edge1 && edge1 < proxy1.Count);
// Get the normal of the reference edge in proxy2's frame.
Vector2 normal1 = proxy1.Normals[edge1];
// Find the incident edge on proxy2.
int index = 0;
float minDot = float.MaxValue;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, proxy2.Normals[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;
ClipVertex cTemp = new ClipVertex();
cTemp.V = proxy2.Vertices[i1];
cTemp.ID.Features.IndexA = (byte)edge1;
cTemp.ID.Features.IndexB = (byte)i1;
cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
c[0] = cTemp;
cTemp.V = proxy2.Vertices[i2];
cTemp.ID.Features.IndexA = (byte)edge1;
cTemp.ID.Features.IndexB = (byte)i2;
cTemp.ID.Features.TypeA = (byte)ContactFeatureType.Face;
cTemp.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
c[1] = cTemp;
}
public void Initialize()
{
Points = FixedArray2 <ManifoldPoint> .Create();
}
private static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
PolygonShape poly1, ref Transform xf1, int edge1,
PolygonShape poly2, ref Transform xf2)
{
c = new FixedArray2<ClipVertex>();
int count2 = poly2.Vertices.Count;
Debug.Assert(0 <= edge1 && edge1 < poly1.Vertices.Count);
// Get the normal of the reference edge in poly2's frame.
Vector2 v = poly1.Normals[edge1];
float tmpx = xf1.R.Col1.X * v.X + xf1.R.Col2.X * v.Y;
float tmpy = xf1.R.Col1.Y * v.X + xf1.R.Col2.Y * v.Y;
Vector2 normal1 = new Vector2(tmpx * xf2.R.Col1.X + tmpy * xf2.R.Col1.Y,
tmpx * xf2.R.Col2.X + tmpy * xf2.R.Col2.Y);
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.MaxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, poly2.Normals[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;
ClipVertex cv0 = c[0];
Vector2 v1 = poly2.Vertices[i1];
cv0.V.X = xf2.Position.X + xf2.R.Col1.X * v1.X + xf2.R.Col2.X * v1.Y;
cv0.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v1.X + xf2.R.Col2.Y * v1.Y;
cv0.ID.Features.IndexA = (byte)edge1;
cv0.ID.Features.IndexB = (byte)i1;
cv0.ID.Features.TypeA = (byte)ContactFeatureType.Face;
cv0.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
c[0] = cv0;
ClipVertex cv1 = c[1];
Vector2 v2 = poly2.Vertices[i2];
cv1.V.X = xf2.Position.X + xf2.R.Col1.X * v2.X + xf2.R.Col2.X * v2.Y;
cv1.V.Y = xf2.Position.Y + xf2.R.Col1.Y * v2.X + xf2.R.Col2.Y * v2.Y;
cv1.ID.Features.IndexA = (byte)edge1;
cv1.ID.Features.IndexB = (byte)i2;
cv1.ID.Features.TypeA = (byte)ContactFeatureType.Face;
cv1.ID.Features.TypeB = (byte)ContactFeatureType.Vertex;
c[1] = cv1;
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void GetWorldManifold(out Vector2 normal, out FixedArray2<Vector2> points)
{
Body bodyA = FixtureA.Body;
Body bodyB = FixtureB.Body;
Shape shapeA = FixtureA.Shape;
Shape shapeB = FixtureB.Shape;
Collision.Collision.GetWorldManifold(ref Manifold, ref bodyA.Xf, shapeA.Radius, ref bodyB.Xf, shapeB.Radius,
out normal, out points);
}
/// <summary>
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the Shapes
/// that generated the manifold.
/// </summary>
public static void Initialize(ref Manifold manifold, ref Transform xfA, float radiusA, ref Transform xfB, float radiusB, out Vector2 normal, out FixedArray2 <Vector2> points, out FixedArray2 <float> separations)
{
normal = Vector2.Zero;
points = new FixedArray2 <Vector2>();
separations = new FixedArray2 <float>();
if (manifold.PointCount == 0)
{
return;
}
switch (manifold.Type)
{
case ManifoldType.Circles:
{
normal = new Vector2(1.0f, 0.0f);
Vector2 pointA = MathUtils.Mul(ref xfA, manifold.LocalPoint);
Vector2 pointB = MathUtils.Mul(ref xfB, manifold.Points.Value0.LocalPoint);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.Epsilon * Settings.Epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
points.Value0 = 0.5f * (cA + cB);
separations.Value0 = Vector2.Dot(cB - cA, normal);
}
break;
case ManifoldType.FaceA:
{
normal = MathUtils.Mul(xfA.q, manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref xfA, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = MathUtils.Mul(ref xfB, manifold.Points[i].LocalPoint);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = Vector2.Dot(cB - cA, normal);
}
}
break;
case ManifoldType.FaceB:
{
normal = MathUtils.Mul(xfB.q, manifold.LocalNormal);
Vector2 planePoint = MathUtils.Mul(ref xfB, manifold.LocalPoint);
for (int i = 0; i < manifold.PointCount; ++i)
{
Vector2 clipPoint = MathUtils.Mul(ref xfA, manifold.Points[i].LocalPoint);
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = Vector2.Dot(cA - cB, normal);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
public static void CollideEdgeAndPolygon(ref Manifold manifold, EdgeShape edgeA, ref Transform xfA, PolygonShape polygonB, ref Transform xfB)
{
manifold.PointCount = 0;
Transform xf = MathUtils.MulT(xfA, xfB);
Vector2 centroidB = MathUtils.Mul(ref xf, polygonB._massData._centroid);
Vector2 v1 = edgeA._vertex1;
Vector2 v2 = edgeA._vertex2;
Vector2 edge1 = v2 - v1;
edge1.Normalize();
// Normal points to the right for a CCW winding
Vector2 normal1 = new Vector2(edge1.Y, -edge1.X);
float offset1 = MathUtils.Dot(normal1, centroidB - v1);
bool oneSided = edgeA._oneSided;
if (oneSided && offset1 < 0.0f)
{
return;
}
// Get polygonB in frameA
TempPolygon tempPolygonB = new TempPolygon(polygonB._vertices.Count);
for (int i = 0; i < polygonB._vertices.Count; ++i)
{
tempPolygonB.Vertices[i] = MathUtils.Mul(ref xf, polygonB._vertices[i]);
tempPolygonB.Normals[i] = MathUtils.Mul(xf.q, polygonB._normals[i]);
}
float radius = polygonB._radius + edgeA._radius;
EPAxis edgeAxis = ComputeEdgeSeparation(ref tempPolygonB, v1, normal1);
if (edgeAxis.Separation > radius)
{
return;
}
EPAxis polygonAxis = ComputePolygonSeparation(ref tempPolygonB, v1, v2);
if (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.Separation - radius > k_relativeTol * (edgeAxis.Separation - radius) + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
if (oneSided)
{
// Smooth collision
// See https://box2d.org/posts/2020/06/ghost-collisions/
Vector2 edge0 = v1 - edgeA._vertex0;
edge0.Normalize();
Vector2 normal0 = new Vector2(edge0.Y, -edge0.X);
bool convex1 = MathUtils.Cross(edge0, edge1) >= 0.0f;
Vector2 edge2 = edgeA._vertex3 - v2;
edge2.Normalize();
Vector2 normal2 = new Vector2(edge2.Y, -edge2.X);
bool convex2 = MathUtils.Cross(edge1, edge2) >= 0.0f;
const float sinTol = 0.1f;
bool side1 = MathUtils.Dot(primaryAxis.Normal, edge1) <= 0.0f;
// Check Gauss Map
if (side1)
{
if (convex1)
{
if (MathUtils.Cross(primaryAxis.Normal, normal0) > sinTol)
{
// Skip region
return;
}
// Admit region
}
else
{
// Snap region
primaryAxis = edgeAxis;
}
}
else
{
if (convex2)
{
if (MathUtils.Cross(normal2, primaryAxis.Normal) > sinTol)
{
// Skip region
return;
}
// Admit region
}
else
{
// Snap region
primaryAxis = edgeAxis;
}
}
}
FixedArray2 <ClipVertex> clipPoints = new FixedArray2 <ClipVertex>();
ReferenceFace ref1;
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.Type = ManifoldType.FaceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int bestIndex = 0;
float bestValue = MathUtils.Dot(primaryAxis.Normal, tempPolygonB.Normals[0]);
for (int i = 1; i < tempPolygonB.Count; ++i)
{
float value = MathUtils.Dot(primaryAxis.Normal, tempPolygonB.Normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int i1 = bestIndex;
int i2 = i1 + 1 < tempPolygonB.Count ? i1 + 1 : 0;
clipPoints.Value0.V = tempPolygonB.Vertices[i1];
clipPoints.Value0.Id.ContactFeature.IndexA = 0;
clipPoints.Value0.Id.ContactFeature.IndexB = (byte)i1;
clipPoints.Value0.Id.ContactFeature.TypeA = ContactFeatureType.Face;
clipPoints.Value0.Id.ContactFeature.TypeB = ContactFeatureType.Vertex;
clipPoints.Value1.V = tempPolygonB.Vertices[i2];
clipPoints.Value1.Id.ContactFeature.IndexA = 0;
clipPoints.Value1.Id.ContactFeature.IndexB = (byte)i2;
clipPoints.Value1.Id.ContactFeature.TypeA = ContactFeatureType.Face;
clipPoints.Value1.Id.ContactFeature.TypeB = ContactFeatureType.Vertex;
ref1.i1 = 0;
ref1.i2 = 1;
ref1.v1 = v1;
ref1.v2 = v2;
ref1.Normal = primaryAxis.Normal;
ref1.SideNormal1 = -edge1;
ref1.SideNormal2 = edge1;
}
else
{
manifold.Type = ManifoldType.FaceB;
clipPoints.Value0.V = v2;
clipPoints.Value0.Id.ContactFeature.IndexA = 1;
clipPoints.Value0.Id.ContactFeature.IndexB = (byte)primaryAxis.Index;
clipPoints.Value0.Id.ContactFeature.TypeA = ContactFeatureType.Vertex;
clipPoints.Value0.Id.ContactFeature.TypeB = ContactFeatureType.Face;
clipPoints.Value1.V = v1;
clipPoints.Value1.Id.ContactFeature.IndexA = 0;
clipPoints.Value1.Id.ContactFeature.IndexB = (byte)primaryAxis.Index;
clipPoints.Value1.Id.ContactFeature.TypeA = ContactFeatureType.Vertex;
clipPoints.Value1.Id.ContactFeature.TypeB = ContactFeatureType.Face;
ref1.i1 = primaryAxis.Index;
ref1.i2 = ref1.i1 + 1 < tempPolygonB.Count ? ref1.i1 + 1 : 0;
ref1.v1 = tempPolygonB.Vertices[ref1.i1];
ref1.v2 = tempPolygonB.Vertices[ref1.i2];
ref1.Normal = tempPolygonB.Normals[ref1.i1];
// CCW winding
ref1.SideNormal1 = new Vector2(ref1.Normal.Y, -ref1.Normal.X);
ref1.SideNormal2 = -ref1.SideNormal1;
}
ref1.SideOffset1 = MathUtils.Dot(ref1.SideNormal1, ref1.v1);
ref1.SideOffset2 = MathUtils.Dot(ref1.SideNormal2, ref1.v2);
// Clip incident edge against reference face side planes
FixedArray2 <ClipVertex> clipPoints1;
FixedArray2 <ClipVertex> clipPoints2;
int np;
// Clip to side 1
np = Collision.ClipSegmentToLine(out clipPoints1, ref clipPoints, ref1.SideNormal1, ref1.SideOffset1, ref1.i1);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Clip to side 2
np = Collision.ClipSegmentToLine(out clipPoints2, ref clipPoints1, ref1.SideNormal2, ref1.SideOffset2, ref1.i2);
if (np < Settings.MaxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.Type == EPAxisType.EdgeA)
{
manifold.LocalNormal = ref1.Normal;
manifold.LocalPoint = ref1.v1;
}
else
{
manifold.LocalNormal = polygonB._normals[ref1.i1];
manifold.LocalPoint = polygonB._vertices[ref1.i1];
}
int pointCount = 0;
for (int i = 0; i < Settings.MaxManifoldPoints; ++i)
{
float separation = MathUtils.Dot(ref1.Normal, clipPoints2[i].V - ref1.v1);
if (separation <= radius)
{
ManifoldPoint cp = manifold.Points[pointCount];
if (primaryAxis.Type == EPAxisType.EdgeA)
{
cp.LocalPoint = MathUtils.MulT(xf, clipPoints2[i].V);
cp.Id = clipPoints2[i].Id;
}
else
{
cp.LocalPoint = clipPoints2[i].V;
cp.Id.ContactFeature.TypeA = clipPoints2[i].Id.ContactFeature.TypeB;
cp.Id.ContactFeature.TypeB = clipPoints2[i].Id.ContactFeature.TypeA;
cp.Id.ContactFeature.IndexA = clipPoints2[i].Id.ContactFeature.IndexB;
cp.Id.ContactFeature.IndexB = clipPoints2[i].Id.ContactFeature.IndexA;
}
manifold.Points[pointCount] = cp;
++pointCount;
}
}
manifold.PointCount = pointCount;
}
/// <summary>
/// Clipping for contact manifolds.
/// </summary>
/// <param name="vOut"></param>
/// <param name="vIn"></param>
/// <param name="normal"></param>
/// <param name="offset"></param>
/// <returns></returns>
public static int ClipSegmentToLine(out FixedArray2<ClipVertex> vOut, ref FixedArray2<ClipVertex> vIn,
Vector2 normal, float offset)
{
vOut = new FixedArray2<ClipVertex>();
// Start with no output points
int numOut = 0;
// Calculate the distance of end points to the line
float distance0 = Vector2.Dot(normal, vIn[0].v) - offset;
float distance1 = Vector2.Dot(normal, vIn[1].v) - offset;
// If the points are behind the plane
if (distance0 <= 0.0f) vOut[numOut++] = vIn[0];
if (distance1 <= 0.0f) vOut[numOut++] = vIn[1];
// If the points are on different sides of the plane
if (distance0 * distance1 < 0.0f)
{
// Find intersection point of edge and plane
float interp = distance0 / (distance0 - distance1);
var cv = vOut[numOut];
cv.v = vIn[0].v + interp * (vIn[1].v - vIn[0].v);
if (distance0 > 0.0f)
{
cv.id = vIn[0].id;
}
else
{
cv.id = vIn[1].id;
}
vOut[numOut] = cv;
++numOut;
}
return numOut;
}
public FixedArray2<Vector2> _points; ///< world contact point (point of intersection)
#endregion Fields
#region Constructors
/// Evaluate the manifold with supplied transforms. This assumes
/// modest motion from the original state. This does not change the
/// point count, impulses, etc. The radii must come from the shapes
/// that generated the manifold.
public WorldManifold(ref Manifold manifold,
ref XForm xfA, float radiusA,
ref XForm xfB, float radiusB)
{
_normal = Vector2.Zero;
_points = new FixedArray2<Vector2>();
if (manifold._pointCount == 0)
{
return;
}
switch (manifold._type)
{
case ManifoldType.Circles:
{
Vector2 pointA = MathUtils.Multiply(ref xfA, manifold._localPoint);
Vector2 pointB = MathUtils.Multiply(ref xfB, manifold._points[0].LocalPoint);
Vector2 normal = new Vector2(1.0f, 0.0f);
if (Vector2.DistanceSquared(pointA, pointB) > Settings.b2_FLT_EPSILON * Settings.b2_FLT_EPSILON)
{
normal = pointB - pointA;
normal.Normalize();
}
_normal = normal;
Vector2 cA = pointA + radiusA * normal;
Vector2 cB = pointB - radiusB * normal;
_points[0] = 0.5f * (cA + cB);
}
break;
case ManifoldType.FaceA:
{
Vector2 normal = MathUtils.Multiply(ref xfA.R, manifold._localPlaneNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfA, manifold._localPoint);
// Ensure normal points from A to B.
_normal = normal;
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfB, manifold._points[i].LocalPoint);
Vector2 cA = clipPoint + (radiusA - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
Vector2 cB = clipPoint - radiusB * normal;
_points[i] = 0.5f * (cA + cB);
}
}
break;
case ManifoldType.FaceB:
{
Vector2 normal = MathUtils.Multiply(ref xfB.R, manifold._localPlaneNormal);
Vector2 planePoint = MathUtils.Multiply(ref xfB, manifold._localPoint);
// Ensure normal points from A to B.
_normal = -normal;
for (int i = 0; i < manifold._pointCount; ++i)
{
Vector2 clipPoint = MathUtils.Multiply(ref xfA, manifold._points[i].LocalPoint);
Vector2 cA = clipPoint - radiusA * normal;
Vector2 cB = clipPoint + (radiusB - Vector2.Dot(clipPoint - planePoint, normal)) * normal;
_points[i] = 0.5f * (cA + cB);
}
}
break;
}
}
static void FindIncidentEdge(out FixedArray2<ClipVertex> c,
PolygonShape poly1, ref Transform xf1, int edge1,
PolygonShape poly2, ref Transform xf2)
{
c = new FixedArray2<ClipVertex>();
int count1 = poly1._vertexCount;
int count2 = poly2._vertexCount;
Debug.Assert(0 <= edge1 && edge1 < count1);
// Get the normal of the reference edge in poly2's frame.
Vector2 normal1 = MathUtils.MultiplyT(ref xf2.R, MathUtils.Multiply(ref xf1.R, poly1._normals[edge1]));
// Find the incident edge on poly2.
int index = 0;
float minDot = Settings.b2_maxFloat;
for (int i = 0; i < count2; ++i)
{
float dot = Vector2.Dot(normal1, poly2._normals[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;
var cv0 = c[0];
cv0.v = MathUtils.Multiply(ref xf2, poly2._vertices[i1]);
cv0.id.Features.ReferenceEdge = (byte)edge1;
cv0.id.Features.IncidentEdge = (byte)i1;
cv0.id.Features.IncidentVertex = 0;
c[0] = cv0;
var cv1 = c[1];
cv1.v = MathUtils.Multiply(ref xf2, poly2._vertices[i2]);
cv1.id.Features.ReferenceEdge = (byte)edge1;
cv1.id.Features.IncidentEdge = (byte)i2;
cv1.id.Features.IncidentVertex = 1;
c[1] = cv1;
}
private void AfterLowerShapeCollision(Fixture fA, Fixture fB, Contact contact)
{
var v = new Vector2();
var fa = new FixedArray2<Vector2>();
contact.GetWorldManifold(out v, out fa);
v = fa[0];
_groundContactPoint = v;
if (v.Y > Body.Position.Y + 1.1f)
IsTouchingGround = true;
contact.Enabled = true;
}
/// <summary>
/// Gets the world manifold.
/// </summary>
public void getWorldManifold( out Vector2 normal, out FixedArray2<Vector2> points )
{
var bodyA = fixtureA.body;
var bodyB = fixtureB.body;
var shapeA = fixtureA.shape;
var shapeB = fixtureB.shape;
ContactSolver.WorldManifold.initialize( ref manifold, ref bodyA._xf, shapeA.radius, ref bodyB._xf, shapeB.radius, out normal, out points );
}
private void AfterUpperShapeCollision(Fixture fA, Fixture fB, Contact contact)
{
var v = new Vector2();
var fa = new FixedArray2<Vector2>();
contact.GetWorldManifold(out v, out fa);
v = fa[0];
if (v.X < Body.Position.X - 0.5f)
IsTouchingLeftWall = true;
else if (v.X > Body.Position.X + 0.5f)
IsTouchingRightWall = true;
contact.Enabled = true;
}