b2MouseJoint::b2MouseJoint(const b2MouseJointDef* def)
: b2Joint(def)
{
b2Assert(def->target.IsValid());
b2Assert(b2IsValid(def->maxForce) && def->maxForce >= 0.0f);
b2Assert(b2IsValid(def->frequencyHz) && def->frequencyHz >= 0.0f);
b2Assert(b2IsValid(def->dampingRatio) && def->dampingRatio >= 0.0f);
m_targetA = def->target;
m_localAnchorB = b2MulT(m_bodyB->GetTransform(), m_targetA);
m_maxForce = def->maxForce;
m_impulse.SetZero();
m_frequencyHz = def->frequencyHz;
m_dampingRatio = def->dampingRatio;
m_beta = 0.0f;
m_gamma = 0.0f;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float32 b2FindMaxSeparation(int32* edgeIndex,
const b2PolygonShape* poly1, const b2Transform& xf1,
const b2PolygonShape* poly2, const b2Transform& xf2)
{
int32 count1 = poly1->m_count;
int32 count2 = poly2->m_count;
const b2Vec2* n1s = poly1->m_normals;
const b2Vec2* v1s = poly1->m_vertices;
const b2Vec2* v2s = poly2->m_vertices;
b2Transform xf = b2MulT(xf2, xf1);
int32 bestIndex = 0;
float32 maxSeparation = -b2_maxFloat;
for (int32 i = 0; i < count1; ++i)
{
// Get poly1 normal in frame2.
b2Vec2 n = b2Mul(xf.q, n1s[i]);
b2Vec2 v1 = b2Mul(xf, v1s[i]);
// Find deepest point for normal i.
float32 si = b2_maxFloat;
for (int32 j = 0; j < count2; ++j)
{
float32 sij = b2Dot(n, v2s[j] - v1);
if (sij < si)
{
si = sij;
}
}
if (si > maxSeparation)
{
maxSeparation = si;
bestIndex = i;
}
}
*edgeIndex = bestIndex;
return maxSeparation;
}
b2EPCollider::b2EPCollider(const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB, const b2Transform& xfB)
{
m_xf = b2MulT(xfA, xfB);
// Create a polygon for edge shape A
m_polygonA.SetAsEdge(edgeA->m_vertex1, edgeA->m_vertex2);
// Build polygonB in frame A
m_polygonB.m_radius = polygonB->m_radius;
m_polygonB.m_vertexCount = polygonB->m_vertexCount;
m_polygonB.m_centroid = b2Mul(m_xf, polygonB->m_centroid);
for (int32 i = 0; i < m_polygonB.m_vertexCount; ++i)
{
m_polygonB.m_vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
m_polygonB.m_normals[i] = b2Mul(m_xf.R, polygonB->m_normals[i]);
}
m_radius = m_polygonA.m_radius + m_polygonB.m_radius;
// Edge geometry
m_edgeA.v0 = edgeA->m_vertex0;
m_edgeA.v1 = edgeA->m_vertex1;
m_edgeA.v2 = edgeA->m_vertex2;
m_edgeA.v3 = edgeA->m_vertex3;
b2Vec2 e = m_edgeA.v2 - m_edgeA.v1;
// Normal points outwards in CCW order.
m_edgeA.normal.Set(e.y, -e.x);
m_edgeA.normal.Normalize();
m_edgeA.hasVertex0 = edgeA->m_hasVertex0;
m_edgeA.hasVertex3 = edgeA->m_hasVertex3;
m_limit11.SetZero();
m_limit12.SetZero();
m_limit21.SetZero();
m_limit22.SetZero();
}
BEGIN_BOX2D_NSP
// Find the separation between poly1 and poly2 for a give edge normal on poly1.
static float32 b2EdgeSeparation(const b2PolygonShape* poly1, const b2Transform& xf1, int32 edge1,
const b2PolygonShape* poly2, const b2Transform& xf2)
{
const b2Vec2* vertices1 = poly1->m_vertices;
const b2Vec2* normals1 = poly1->m_normals;
int32 count2 = poly2->m_vertexCount;
const b2Vec2* vertices2 = poly2->m_vertices;
b2Assert(0 <= edge1 && edge1 < poly1->m_vertexCount);
// Convert normal from poly1's frame into poly2's frame.
b2Vec2 normal1World = b2Mul(xf1.q, normals1[edge1]);
b2Vec2 normal1 = b2MulT(xf2.q, normal1World);
// Find support vertex on poly2 for -normal.
int32 index = 0;
float32 minDot = b2_maxFloat;
for (int32 i = 0; i < count2; ++i)
{
float32 dot = b2Dot(vertices2[i], normal1);
if (dot < minDot)
{
minDot = dot;
index = i;
}
}
b2Vec2 v1 = b2Mul(xf1, vertices1[edge1]);
b2Vec2 v2 = b2Mul(xf2, vertices2[index]);
float32 separation = b2Dot(v2 - v1, normal1World);
return separation;
}
// Compute contact points for edge versus circle.
// This accounts for edge connectivity.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
// Compute circle in frame of edge
b2Vec2 Q = b2MulT(xfA, b2Mul(xfB, circleB->m_p));
b2Vec2 A = edgeA->m_vertex1, B = edgeA->m_vertex2;
b2Vec2 e = B - A;
// Barycentric coordinates
float32 u = b2Dot(e, B - Q);
float32 v = b2Dot(e, Q - A);
float32 radius = edgeA->m_radius + circleB->m_radius;
b2ContactFeature cf;
cf.indexB = 0;
cf.typeB = b2ContactFeature::e_vertex;
// Region A
if (v <= 0.0f)
{
b2Vec2 P = A;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to A?
if (edgeA->m_hasVertex0)
{
b2Vec2 A1 = edgeA->m_vertex0;
b2Vec2 B1 = A;
b2Vec2 e1 = B1 - A1;
float32 u1 = b2Dot(e1, B1 - Q);
// Is the circle in Region AB of the previous edge?
if (u1 > 0.0f)
{
return;
}
}
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region B
if (u <= 0.0f)
{
b2Vec2 P = B;
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
// Is there an edge connected to B?
if (edgeA->m_hasVertex3)
{
b2Vec2 B2 = edgeA->m_vertex3;
b2Vec2 A2 = B;
b2Vec2 e2 = B2 - A2;
float32 v2 = b2Dot(e2, Q - A2);
// Is the circle in Region AB of the next edge?
if (v2 > 0.0f)
{
return;
}
}
cf.indexA = 1;
cf.typeA = b2ContactFeature::e_vertex;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_circles;
manifold->localNormal.SetZero();
manifold->localPoint = P;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
return;
}
// Region AB
float32 den = b2Dot(e, e);
b2Assert(den > 0.0f);
b2Vec2 P = (1.0f / den) * (u * A + v * B);
b2Vec2 d = Q - P;
float32 dd = b2Dot(d, d);
if (dd > radius * radius)
{
return;
}
b2Vec2 n(-e.y, e.x);
if (b2Dot(n, Q - A) < 0.0f)
{
n.Set(-n.x, -n.y);
}
n.Normalize();
cf.indexA = 0;
cf.typeA = b2ContactFeature::e_face;
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = n;
manifold->localPoint = A;
manifold->points[0].id.key = 0;
manifold->points[0].id.cf = cf;
manifold->points[0].localPoint = circleB->m_p;
}
float32 FindMinSeparation(int32* indexA, int32* indexB, float32 t) const
{
b2Transform xfA, xfB;
m_sweepA.GetTransform(&xfA, t);
m_sweepB.GetTransform(&xfB, t);
switch (m_type)
{
case e_points:
{
b2Vec2 axisA = b2MulT(xfA.R, m_axis);
b2Vec2 axisB = b2MulT(xfB.R, -m_axis);
*indexA = m_proxyA->GetSupport(axisA);
*indexB = m_proxyB->GetSupport(axisB);
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointA = b2Mul(xfA, localPointA);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, m_axis);
return separation;
}
case e_faceA:
{
b2Vec2 normal = b2Mul(xfA.R, m_axis);
b2Vec2 pointA = b2Mul(xfA, m_localPoint);
b2Vec2 axisB = b2MulT(xfB.R, -normal);
*indexA = -1;
*indexB = m_proxyB->GetSupport(axisB);
b2Vec2 localPointB = m_proxyB->GetVertex(*indexB);
b2Vec2 pointB = b2Mul(xfB, localPointB);
float32 separation = b2Dot(pointB - pointA, normal);
return separation;
}
case e_faceB:
{
b2Vec2 normal = b2Mul(xfB.R, m_axis);
b2Vec2 pointB = b2Mul(xfB, m_localPoint);
b2Vec2 axisA = b2MulT(xfA.R, -normal);
*indexB = -1;
*indexA = m_proxyA->GetSupport(axisA);
b2Vec2 localPointA = m_proxyA->GetVertex(*indexA);
b2Vec2 pointA = b2Mul(xfA, localPointA);
float32 separation = b2Dot(pointA - pointB, normal);
return separation;
}
default:
b2Assert(false);
*indexA = -1;
*indexB = -1;
return 0.0f;
}
}
// Collide an edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
// to allow the polygon to slide smoothly over an edge chain.
//
// Algorithm
// 1. Classify front-side or back-side collision with edge.
// 2. Compute separation
// 3. Process adjacent edges
// 4. Classify adjacent edge as convex, flat, null, or concave
// 5. Skip null or concave edges. Concave edges get a separate manifold.
// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
// 7. If the edge is convex, compute it's separation.
// 8. Use the minimum separation of up to three edges. If the minimum separation
// is not the primary edge, return.
// 9. If the minimum separation is the primary edge, compute the contact points and return.
void b2EPCollider::Collide(b2Manifold* manifold)
{
manifold->pointCount = 0;
ComputeAdjacency();
b2EPAxis 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 == b2EPAxis::e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxis::e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2EPProxy* proxy1;
b2EPProxy* proxy2;
b2ClipVertex incidentEdge[2];
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
proxy1 = &m_proxyA;
proxy2 = &m_proxyB;
manifold->type = b2Manifold::e_faceA;
}
else
{
proxy1 = &m_proxyB;
proxy2 = &m_proxyA;
manifold->type = b2Manifold::e_faceB;
}
int32 edge1 = primaryAxis.index;
FindIncidentEdge(incidentEdge, proxy1, primaryAxis.index, proxy2);
int32 count1 = proxy1->count;
const b2Vec2* vertices1 = proxy1->vertices;
int32 iv1 = edge1;
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 tangent = v12 - v11;
tangent.Normalize();
b2Vec2 normal = b2Cross(tangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + m_radius;
float32 sideOffset2 = b2Dot(tangent, v12) + m_radius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->localNormal = normal;
manifold->localPoint = planePoint;
}
else
{
manifold->localNormal = b2MulT(m_xf.R, normal);
manifold->localPoint = b2MulT(m_xf, planePoint);
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= m_radius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
}
else
{
cp->localPoint = clipPoints2[i].v;
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
// 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
void b2EPCollider::Collide(b2Manifold* manifold, const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB, const b2Transform& xfB)
{
m_xf = b2MulT(xfA, xfB);
m_centroidB = b2Mul(m_xf, polygonB->m_centroid);
m_v0 = edgeA->m_vertex0;
m_v1 = edgeA->m_vertex1;
m_v2 = edgeA->m_vertex2;
m_v3 = edgeA->m_vertex3;
bool hasVertex0 = edgeA->m_hasVertex0;
bool hasVertex3 = edgeA->m_hasVertex3;
b2Vec2 edge1 = m_v2 - m_v1;
edge1.Normalize();
m_normal1.Set(edge1.y, -edge1.x);
float32 offset1 = b2Dot(m_normal1, m_centroidB - m_v1);
float32 offset0 = 0.0f, offset2 = 0.0f;
bool convex1 = false, convex2 = false;
// Is there a preceding edge?
if (hasVertex0)
{
b2Vec2 edge0 = m_v1 - m_v0;
edge0.Normalize();
m_normal0.Set(edge0.y, -edge0.x);
convex1 = b2Cross(edge0, edge1) >= 0.0f;
offset0 = b2Dot(m_normal0, m_centroidB - m_v0);
}
// Is there a following edge?
if (hasVertex3)
{
b2Vec2 edge2 = m_v3 - m_v2;
edge2.Normalize();
m_normal2.Set(edge2.y, -edge2.x);
convex2 = b2Cross(edge1, edge2) > 0.0f;
offset2 = b2Dot(m_normal2, m_centroidB - m_v2);
}
// Determine front or back collision. Determine collision normal limits.
if (hasVertex0 && hasVertex3)
{
if (convex1 && convex2)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
}
else if (convex1)
{
m_front = offset0 >= 0.0f || (offset1 >= 0.0f && offset2 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal1;
}
}
else if (convex2)
{
m_front = offset2 >= 0.0f || (offset0 >= 0.0f && offset1 >= 0.0f);
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal0;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex0)
{
if (convex1)
{
m_front = offset0 >= 0.0f || offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal0;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
}
else
{
m_front = offset0 >= 0.0f && offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = -m_normal0;
}
}
}
else if (hasVertex3)
{
if (convex2)
{
m_front = offset1 >= 0.0f || offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal2;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
}
else
{
m_front = offset1 >= 0.0f && offset2 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = -m_normal2;
m_upperLimit = m_normal1;
}
}
}
else
{
m_front = offset1 >= 0.0f;
if (m_front)
{
m_normal = m_normal1;
m_lowerLimit = -m_normal1;
m_upperLimit = -m_normal1;
}
else
{
m_normal = -m_normal1;
m_lowerLimit = m_normal1;
m_upperLimit = m_normal1;
}
}
// Get polygonB in frameA
m_polygonB.count = polygonB->m_vertexCount;
for (int32 i = 0; i < polygonB->m_vertexCount; ++i)
{
m_polygonB.vertices[i] = b2Mul(m_xf, polygonB->m_vertices[i]);
m_polygonB.normals[i] = b2Mul(m_xf.q, polygonB->m_normals[i]);
}
m_radius = 2.0f * b2_polygonRadius;
manifold->pointCount = 0;
b2EPAxis edgeAxis = ComputeEdgeSeparation();
// If no valid normal can be found than this edge should not collide.
if (edgeAxis.type == b2EPAxis::e_unknown)
{
return;
}
if (edgeAxis.separation > m_radius)
{
return;
}
b2EPAxis polygonAxis = ComputePolygonSeparation();
if (polygonAxis.type != b2EPAxis::e_unknown && polygonAxis.separation > m_radius)
{
return;
}
// Use hysteresis for jitter reduction.
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.type == b2EPAxis::e_unknown)
{
primaryAxis = edgeAxis;
}
else if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2ClipVertex ie[2];
b2ReferenceFace rf;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->type = b2Manifold::e_faceA;
// Search for the polygon normal that is most anti-parallel to the edge normal.
int32 bestIndex = 0;
float32 bestValue = b2Dot(m_normal, m_polygonB.normals[0]);
for (int32 i = 1; i < m_polygonB.count; ++i)
{
float32 value = b2Dot(m_normal, m_polygonB.normals[i]);
if (value < bestValue)
{
bestValue = value;
bestIndex = i;
}
}
int32 i1 = bestIndex;
int32 i2 = i1 + 1 < m_polygonB.count ? i1 + 1 : 0;
ie[0].v = m_polygonB.vertices[i1];
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = i1;
ie[0].id.cf.typeA = b2ContactFeature::e_face;
ie[0].id.cf.typeB = b2ContactFeature::e_vertex;
ie[1].v = m_polygonB.vertices[i2];
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = i2;
ie[1].id.cf.typeA = b2ContactFeature::e_face;
ie[1].id.cf.typeB = b2ContactFeature::e_vertex;
if (m_front)
{
rf.i1 = 0;
rf.i2 = 1;
rf.v1 = m_v1;
rf.v2 = m_v2;
rf.normal = m_normal1;
}
else
{
rf.i1 = 1;
rf.i2 = 0;
rf.v1 = m_v2;
rf.v2 = m_v1;
rf.normal = -m_normal1;
}
}
else
{
manifold->type = b2Manifold::e_faceB;
ie[0].v = m_v1;
ie[0].id.cf.indexA = 0;
ie[0].id.cf.indexB = primaryAxis.index;
ie[0].id.cf.typeA = b2ContactFeature::e_vertex;
ie[0].id.cf.typeB = b2ContactFeature::e_face;
ie[1].v = m_v2;
ie[1].id.cf.indexA = 0;
ie[1].id.cf.indexB = primaryAxis.index;
ie[1].id.cf.typeA = b2ContactFeature::e_vertex;
ie[1].id.cf.typeB = b2ContactFeature::e_face;
rf.i1 = primaryAxis.index;
rf.i2 = rf.i1 + 1 < m_polygonB.count ? rf.i1 + 1 : 0;
rf.v1 = m_polygonB.vertices[rf.i1];
rf.v2 = m_polygonB.vertices[rf.i2];
rf.normal = m_polygonB.normals[rf.i1];
}
rf.sideNormal1.Set(rf.normal.y, -rf.normal.x);
rf.sideNormal2 = -rf.sideNormal1;
rf.sideOffset1 = b2Dot(rf.sideNormal1, rf.v1);
rf.sideOffset2 = b2Dot(rf.sideNormal2, rf.v2);
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int32 np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, ie, rf.sideNormal1, rf.sideOffset1, rf.i1);
if (np < b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, rf.sideNormal2, rf.sideOffset2, rf.i2);
if (np < b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->localNormal = rf.normal;
manifold->localPoint = rf.v1;
}
else
{
manifold->localNormal = polygonB->m_normals[rf.i1];
manifold->localPoint = polygonB->m_vertices[rf.i1];
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(rf.normal, clipPoints2[i].v - rf.v1);
if (separation <= m_radius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(m_xf, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
}
else
{
cp->localPoint = clipPoints2[i].v;
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
void b2CollidePolygonAndCircle(
b2Manifold* manifold,
const b2PolygonShape* polygonA, const b2Transform& xfA,
const b2CircleShape* circleB, const b2Transform& xfB)
{
manifold->pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Mul(xfB, circleB->m_p);
b2Vec2 cLocal = b2MulT(xfA, c);
// Find the min separating edge.
int32 normalIndex = 0;
float32 separation = -b2_maxFloat;
float32 radius = polygonA->m_radius + circleB->m_radius;
int32 vertexCount = polygonA->m_count;
const b2Vec2* vertices = polygonA->m_vertices;
const b2Vec2* normals = polygonA->m_normals;
for (int32 i = 0; i < vertexCount; ++i)
{
float32 s = b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// Vertices that subtend the incident face.
int32 vertIndex1 = normalIndex;
int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
b2Vec2 v1 = vertices[vertIndex1];
b2Vec2 v2 = vertices[vertIndex2];
// If the center is inside the polygon ...
if (separation < b2_epsilon)
{
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = normals[normalIndex];
manifold->localPoint = 0.5f * (v1 + v2);
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
return;
}
// Compute barycentric coordinates
float32 u1 = b2Dot(cLocal - v1, v2 - v1);
float32 u2 = b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
if (b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = cLocal - v1;
manifold->localNormal.Normalize();
manifold->localPoint = v1;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
else if (u2 <= 0.0f)
{
if (b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = cLocal - v2;
manifold->localNormal.Normalize();
manifold->localPoint = v2;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
else
{
b2Vec2 faceCenter = 0.5f * (v1 + v2);
float32 separation_ = b2Dot(cLocal - faceCenter, normals[vertIndex1]);
if (separation_ > radius)
{
return;
}
manifold->pointCount = 1;
manifold->type = b2Manifold::e_faceA;
manifold->localNormal = normals[vertIndex1];
manifold->localPoint = faceCenter;
manifold->points[0].localPoint = circleB->m_p;
manifold->points[0].id.key = 0;
}
}
float32 b2PolygonShape::ComputeSubmergedArea( const b2Vec2& normal,
float32 offset,
const b2XForm& xf,
b2Vec2* c) const
{
//Transform plane into shape co-ordinates
b2Vec2 normalL = b2MulT(xf.R,normal);
float32 offsetL = offset - b2Dot(normal,xf.position);
float32 depths[b2_maxPolygonVertices];
int32 diveCount = 0;
int32 intoIndex = -1;
int32 outoIndex = -1;
bool lastSubmerged = false;
int32 i;
for(i=0;i<m_vertexCount;i++){
depths[i] = b2Dot(normalL,m_vertices[i]) - offsetL;
bool isSubmerged = depths[i]<-B2_FLT_EPSILON;
if(i>0){
if(isSubmerged){
if(!lastSubmerged){
intoIndex = i-1;
diveCount++;
}
}else{
if(lastSubmerged){
outoIndex = i-1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch(diveCount){
case 0:
if(lastSubmerged){
//Completely submerged
b2MassData md;
ComputeMass(&md);
*c = b2Mul(xf,md.center);
return md.mass/m_density;
}else{
//Completely dry
return 0;
}
break;
case 1:
if(intoIndex==-1){
intoIndex = m_vertexCount-1;
}else{
outoIndex = m_vertexCount-1;
}
break;
}
int32 intoIndex2 = (intoIndex+1)%m_vertexCount;
int32 outoIndex2 = (outoIndex+1)%m_vertexCount;
float32 intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float32 outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
b2Vec2 intoVec( m_vertices[intoIndex].x*(1-intoLambda)+m_vertices[intoIndex2].x*intoLambda,
m_vertices[intoIndex].y*(1-intoLambda)+m_vertices[intoIndex2].y*intoLambda);
b2Vec2 outoVec( m_vertices[outoIndex].x*(1-outoLambda)+m_vertices[outoIndex2].x*outoLambda,
m_vertices[outoIndex].y*(1-outoLambda)+m_vertices[outoIndex2].y*outoLambda);
//Initialize accumulator
float32 area = 0;
b2Vec2 center(0,0);
b2Vec2 p2 = m_vertices[intoIndex2];
b2Vec2 p3;
float32 k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while(i!=outoIndex2){
i=(i+1)%m_vertexCount;
if(i==outoIndex2)
p3 = outoVec;
else
p3 = m_vertices[i];
//Add the triangle formed by intoVec,p2,p3
{
b2Vec2 e1 = p2 - intoVec;
b2Vec2 e2 = p3 - intoVec;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (intoVec + p2 + p3);
}
//
p2=p3;
}
//Normalize and transform centroid
center *= 1.0f/area;
*c = b2Mul(xf,center);
return area;
}
float32 LH_b2BuoyancyController::ComputeSubmergedArea(b2Shape* shape,
const b2Vec2& normal,
float32 offset,
const b2Transform& xf,
b2Vec2* c,
float32 density) const
{
if(shape->GetType() == b2Shape::e_edge)
{
//Note that v0 is independant of any details of the specific edge
//We are relying on v0 being consistent between multiple edges of the same body
b2Vec2 v0 = offset * normal;
//b2Vec2 v0 = xf.position + (offset - b2Dot(normal, xf.position)) * normal;
b2Vec2 v1 = b2Mul(xf, ((b2EdgeShape*)shape)->m_vertex1);
b2Vec2 v2 = b2Mul(xf, ((b2EdgeShape*)shape)->m_vertex2);
float32 d1 = b2Dot(normal, v1) - offset;
float32 d2 = b2Dot(normal, v2) - offset;
if(d1>0)
{
if(d2>0)
{
return 0;
}
else
{
v1 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
}
}
else
{
if(d2>0)
{
v2 = -d2 / (d1 - d2) * v1 + d1 / (d1 - d2) * v2;
}
else
{
//Nothing
}
}
// v0,v1,v2 represents a fully submerged triangle
float32 k_inv3 = 1.0f / 3.0f;
// Area weighted centroid
*c = k_inv3 * (v0 + v1 + v2);
b2Vec2 e1 = v1 - v0;
b2Vec2 e2 = v2 - v0;
return 0.5f * b2Cross(e1, e2);
}
else if(shape->GetType() == b2Shape::e_polygon)
{
//Transform plane into shape co-ordinates
b2Vec2 normalL = b2MulT(xf.q,normal);
float32 offsetL = offset - b2Dot(normal,xf.p);
float32 depths[b2_maxPolygonVertices];
int32 diveCount = 0;
int32 intoIndex = -1;
int32 outoIndex = -1;
bool lastSubmerged = false;
int32 i;
for(i=0;i<((b2PolygonShape*)shape)->m_vertexCount;i++){
depths[i] = b2Dot(normalL,((b2PolygonShape*)shape)->m_vertices[i]) - offsetL;
bool isSubmerged = depths[i]<-FLT_EPSILON;
if(i>0){
if(isSubmerged){
if(!lastSubmerged){
intoIndex = i-1;
diveCount++;
}
}else{
if(lastSubmerged){
outoIndex = i-1;
diveCount++;
}
}
}
lastSubmerged = isSubmerged;
}
switch(diveCount){
case 0:
if(lastSubmerged){
//Completely submerged
b2MassData md;
((b2PolygonShape*)shape)->ComputeMass(&md, density);
*c = b2Mul(xf,md.center);
return md.mass/density;
}else{
//Completely dry
return 0;
}
break;
case 1:
if(intoIndex==-1){
intoIndex = ((b2PolygonShape*)shape)->m_vertexCount-1;
}else{
outoIndex = ((b2PolygonShape*)shape)->m_vertexCount-1;
}
break;
}
int32 intoIndex2 = (intoIndex+1)%((b2PolygonShape*)shape)->m_vertexCount;
int32 outoIndex2 = (outoIndex+1)%((b2PolygonShape*)shape)->m_vertexCount;
float32 intoLambda = (0 - depths[intoIndex]) / (depths[intoIndex2] - depths[intoIndex]);
float32 outoLambda = (0 - depths[outoIndex]) / (depths[outoIndex2] - depths[outoIndex]);
b2Vec2 intoVec( ((b2PolygonShape*)shape)->m_vertices[intoIndex].x*(1-intoLambda)+((b2PolygonShape*)shape)->m_vertices[intoIndex2].x*intoLambda,
((b2PolygonShape*)shape)->m_vertices[intoIndex].y*(1-intoLambda)+((b2PolygonShape*)shape)->m_vertices[intoIndex2].y*intoLambda);
b2Vec2 outoVec( ((b2PolygonShape*)shape)->m_vertices[outoIndex].x*(1-outoLambda)+((b2PolygonShape*)shape)->m_vertices[outoIndex2].x*outoLambda,
((b2PolygonShape*)shape)->m_vertices[outoIndex].y*(1-outoLambda)+((b2PolygonShape*)shape)->m_vertices[outoIndex2].y*outoLambda);
//Initialize accumulator
float32 area = 0;
b2Vec2 center(0,0);
b2Vec2 p2 = ((b2PolygonShape*)shape)->m_vertices[intoIndex2];
b2Vec2 p3;
float32 k_inv3 = 1.0f / 3.0f;
//An awkward loop from intoIndex2+1 to outIndex2
i = intoIndex2;
while(i!=outoIndex2){
i=(i+1)%((b2PolygonShape*)shape)->m_vertexCount;
if(i==outoIndex2)
p3 = outoVec;
else
p3 = ((b2PolygonShape*)shape)->m_vertices[i];
//Add the triangle formed by intoVec,p2,p3
{
b2Vec2 e1 = p2 - intoVec;
b2Vec2 e2 = p3 - intoVec;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
center += triangleArea * k_inv3 * (intoVec + p2 + p3);
}
//
p2=p3;
}
//Normalize and transform centroid
center *= 1.0f/area;
*c = b2Mul(xf,center);
return area;
}
else if(shape->GetType() == b2Shape::e_circle)
{
b2Vec2 p = b2Mul(xf,((b2CircleShape*)shape)->m_p);
float32 l = -(b2Dot(normal,p) - offset);
if(l<-((b2CircleShape*)shape)->m_radius+FLT_EPSILON){
//Completely dry
return 0;
}
if(l > ((b2CircleShape*)shape)->m_radius){
//Completely wet
*c = p;
return b2_pi*((b2CircleShape*)shape)->m_radius*((b2CircleShape*)shape)->m_radius;
}
//Magic
float32 r2 = ((b2CircleShape*)shape)->m_radius*((b2CircleShape*)shape)->m_radius;
float32 l2 = l*l;
//TODO: write b2Sqrt to handle fixed point case.
float32 area = r2 * (asin(l/((b2CircleShape*)shape)->m_radius) + b2_pi/2.0f)+ l * b2Sqrt(r2 - l2);
float32 com = -2.0f/3.0f*pow(r2-l2,1.5f)/area;
c->x = p.x + normal.x * com;
c->y = p.y + normal.y * com;
return area;
}
return 0;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static float32 FindMaxSeparation(int32* edgeIndex,
const b2PolygonShape* poly1, const b2XForm& xf1,
const b2PolygonShape* poly2, const b2XForm& xf2)
{
int32 count1 = poly1->GetVertexCount();
const b2Vec2* normals1 = poly1->GetNormals();
// Vector pointing from the centroid of poly1 to the centroid of poly2.
b2Vec2 d = b2Mul(xf2, poly2->GetCentroid()) - b2Mul(xf1, poly1->GetCentroid());
b2Vec2 dLocal1 = b2MulT(xf1.R, d);
// Find edge normal on poly1 that has the largest projection onto d.
int32 edge = 0;
float32 maxDot = -B2_FLT_MAX;
for (int32 i = 0; i < count1; ++i)
{
float32 dot = b2Dot(normals1[i], dLocal1);
if (dot > maxDot)
{
maxDot = dot;
edge = i;
}
}
// Get the separation for the edge normal.
float32 s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
// Check the separation for the previous edge normal.
int32 prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
float32 sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
if (sPrev > 0.0f)
{
return sPrev;
}
// Check the separation for the next edge normal.
int32 nextEdge = edge + 1 < count1 ? edge + 1 : 0;
float32 sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
if (sNext > 0.0f)
{
return sNext;
}
// Find the best edge and the search direction.
int32 bestEdge;
float32 bestSeparation;
int32 increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
*edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for ( ; ; )
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
*edgeIndex = bestEdge;
return bestSeparation;
}
// Find the max separation between poly1 and poly2 using edge normals from poly1.
static btScalar FindMaxSeparation(int* edgeIndex,
const btBox2dShape* poly1, const btTransform& xf1,
const btBox2dShape* poly2, const btTransform& xf2)
{
int count1 = poly1->getVertexCount();
const btVector3* normals1 = poly1->getNormals();
// Vector pointing from the centroid of poly1 to the centroid of poly2.
btVector3 d = b2Mul(xf2, poly2->getCentroid()) - b2Mul(xf1, poly1->getCentroid());
btVector3 dLocal1 = b2MulT(xf1.getBasis(), d);
// Find edge normal on poly1 that has the largest projection onto d.
int edge = 0;
btScalar maxDot;
if( count1 > 0 )
edge = (int) dLocal1.maxDot( normals1, count1, maxDot);
// Get the separation for the edge normal.
btScalar s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
// Check the separation for the previous edge normal.
int prevEdge = edge - 1 >= 0 ? edge - 1 : count1 - 1;
btScalar sPrev = EdgeSeparation(poly1, xf1, prevEdge, poly2, xf2);
if (sPrev > 0.0f)
{
return sPrev;
}
// Check the separation for the next edge normal.
int nextEdge = edge + 1 < count1 ? edge + 1 : 0;
btScalar sNext = EdgeSeparation(poly1, xf1, nextEdge, poly2, xf2);
if (sNext > 0.0f)
{
return sNext;
}
// Find the best edge and the search direction.
int bestEdge;
btScalar bestSeparation;
int increment;
if (sPrev > s && sPrev > sNext)
{
increment = -1;
bestEdge = prevEdge;
bestSeparation = sPrev;
}
else if (sNext > s)
{
increment = 1;
bestEdge = nextEdge;
bestSeparation = sNext;
}
else
{
*edgeIndex = edge;
return s;
}
// Perform a local search for the best edge normal.
for ( ; ; )
{
if (increment == -1)
edge = bestEdge - 1 >= 0 ? bestEdge - 1 : count1 - 1;
else
edge = bestEdge + 1 < count1 ? bestEdge + 1 : 0;
s = EdgeSeparation(poly1, xf1, edge, poly2, xf2);
if (s > 0.0f)
{
return s;
}
if (s > bestSeparation)
{
bestEdge = edge;
bestSeparation = s;
}
else
{
break;
}
}
*edgeIndex = bestEdge;
return bestSeparation;
}
b2SegmentCollide b2PolygonShape::TestSegment(
const b2XForm& xf,
float32* lambda,
b2Vec2* normal,
const b2Segment& segment,
float32 maxLambda) const
{
float32 lower = 0.0f, upper = maxLambda;
b2Vec2 p1 = b2MulT(xf.R, segment.p1 - xf.position);
b2Vec2 p2 = b2MulT(xf.R, segment.p2 - xf.position);
b2Vec2 d = p2 - p1;
int32 index = -1;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return e_missCollide;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return e_missCollide;
}
}
b2Assert(0.0f <= lower && lower <= maxLambda);
if (index >= 0)
{
*lambda = lower;
*normal = b2Mul(xf.R, m_normals[index]);
return e_hitCollide;
}
*lambda = 0;
return e_startsInsideCollide;
}
void b2CollidePolygonAndCircle(
b2Manifold* manifold,
const b2PolygonShape* polygon, const b2XForm& xf1,
const b2CircleShape* circle, const b2XForm& xf2)
{
manifold->pointCount = 0;
// Compute circle position in the frame of the polygon.
b2Vec2 c = b2Mul(xf2, circle->GetLocalPosition());
b2Vec2 cLocal = b2MulT(xf1, c);
// Find the min separating edge.
int32 normalIndex = 0;
float32 separation = -B2_FLT_MAX;
float32 radius = circle->GetRadius();
int32 vertexCount = polygon->GetVertexCount();
const b2Vec2* vertices = polygon->GetVertices();
const b2Vec2* normals = polygon->GetNormals();
for (int32 i = 0; i < vertexCount; ++i)
{
float32 s = b2Dot(normals[i], cLocal - vertices[i]);
if (s > radius)
{
// Early out.
return;
}
if (s > separation)
{
separation = s;
normalIndex = i;
}
}
// If the center is inside the polygon ...
if (separation < B2_FLT_EPSILON)
{
manifold->pointCount = 1;
manifold->normal = b2Mul(xf1.R, normals[normalIndex]);
manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
manifold->points[0].id.features.incidentVertex = b2_nullFeature;
manifold->points[0].id.features.referenceEdge = 0;
manifold->points[0].id.features.flip = 0;
b2Vec2 position = c - radius * manifold->normal;
manifold->points[0].localPoint1 = b2MulT(xf1, position);
manifold->points[0].localPoint2 = b2MulT(xf2, position);
manifold->points[0].separation = separation - radius;
return;
}
// Project the circle center onto the edge segment.
int32 vertIndex1 = normalIndex;
int32 vertIndex2 = vertIndex1 + 1 < vertexCount ? vertIndex1 + 1 : 0;
b2Vec2 e = vertices[vertIndex2] - vertices[vertIndex1];
float32 length = e.Normalize();
b2Assert(length > B2_FLT_EPSILON);
// Project the center onto the edge.
float32 u = b2Dot(cLocal - vertices[vertIndex1], e);
b2Vec2 p;
if (u <= 0.0f)
{
p = vertices[vertIndex1];
manifold->points[0].id.features.incidentEdge = b2_nullFeature;
manifold->points[0].id.features.incidentVertex = (uint8)vertIndex1;
}
else if (u >= length)
{
p = vertices[vertIndex2];
manifold->points[0].id.features.incidentEdge = b2_nullFeature;
manifold->points[0].id.features.incidentVertex = (uint8)vertIndex2;
}
else
{
p = vertices[vertIndex1] + u * e;
manifold->points[0].id.features.incidentEdge = (uint8)normalIndex;
manifold->points[0].id.features.incidentVertex = b2_nullFeature;
}
b2Vec2 d = cLocal - p;
float32 dist = d.Normalize();
if (dist > radius)
{
return;
}
manifold->pointCount = 1;
manifold->normal = b2Mul(xf1.R, d);
b2Vec2 position = c - radius * manifold->normal;
manifold->points[0].localPoint1 = b2MulT(xf1, position);
manifold->points[0].localPoint2 = b2MulT(xf2, position);
manifold->points[0].separation = dist - radius;
manifold->points[0].id.features.referenceEdge = 0;
manifold->points[0].id.features.flip = 0;
}
void b2CollidePolyAndEdge(b2Manifold* manifold,
const b2PolygonShape* polygon,
const b2XForm& transformA,
const b2EdgeShape* edge,
const b2XForm& transformB)
{
manifold->m_pointCount = 0;
b2Vec2 v1 = b2Mul(transformB, edge->GetVertex1());
b2Vec2 v2 = b2Mul(transformB, edge->GetVertex2());
b2Vec2 n = b2Mul(transformB.R, edge->GetNormalVector());
b2Vec2 v1Local = b2MulT(transformA, v1);
b2Vec2 v2Local = b2MulT(transformA, v2);
b2Vec2 nLocal = b2MulT(transformA.R, n);
float32 totalRadius = polygon->m_radius + edge->m_radius;
float32 separation1;
int32 separationIndex1 = -1; // which normal on the polygon found the shallowest depth?
float32 separationMax1 = -B2_FLT_MAX; // the shallowest depth of edge in polygon
float32 separation2;
int32 separationIndex2 = -1; // which normal on the polygon found the shallowest depth?
float32 separationMax2 = -B2_FLT_MAX; // the shallowest depth of edge in polygon
float32 separationMax = -B2_FLT_MAX; // the shallowest depth of edge in polygon
bool separationV1 = false; // is the shallowest depth from edge's v1 or v2 vertex?
int32 separationIndex = -1; // which normal on the polygon found the shallowest depth?
int32 vertexCount = polygon->m_vertexCount;
const b2Vec2* vertices = polygon->m_vertices;
const b2Vec2* normals = polygon->m_normals;
int32 enterStartIndex = -1; // the last polygon vertex above the edge
int32 enterEndIndex = -1; // the first polygon vertex below the edge
int32 exitStartIndex = -1; // the last polygon vertex below the edge
int32 exitEndIndex = -1; // the first polygon vertex above the edge
//int32 deepestIndex;
// the "N" in the following variables refers to the edge's normal.
// these are projections of polygon vertices along the edge's normal,
// a.k.a. they are the separation of the polygon from the edge.
float32 prevSepN = totalRadius;
float32 nextSepN = totalRadius;
float32 enterSepN = totalRadius; // the depth of enterEndIndex under the edge (stored as a separation, so it's negative)
float32 exitSepN = totalRadius; // the depth of exitStartIndex under the edge (stored as a separation, so it's negative)
float32 deepestSepN = B2_FLT_MAX; // the depth of the deepest polygon vertex under the end (stored as a separation, so it's negative)
// for each polygon normal, get the edge's depth into the polygon.
// for each polygon vertex, get the vertex's depth into the edge.
// use these calculations to define the remaining variables declared above.
prevSepN = b2Dot(vertices[vertexCount-1] - v1Local, nLocal);
for (int32 i = 0; i < vertexCount; i++)
{
// Polygon normal separation.
separation1 = b2Dot(v1Local - vertices[i], normals[i]);
separation2 = b2Dot(v2Local - vertices[i], normals[i]);
if (separation2 < separation1)
{
if (separation2 > separationMax)
{
separationMax = separation2;
separationV1 = false;
separationIndex = i;
}
}
else
{
if (separation1 > separationMax)
{
separationMax = separation1;
separationV1 = true;
separationIndex = i;
}
}
if (separation1 > separationMax1)
{
separationMax1 = separation1;
separationIndex1 = i;
}
if (separation2 > separationMax2)
{
separationMax2 = separation2;
separationIndex2 = i;
}
// Edge normal separation
nextSepN = b2Dot(vertices[i] - v1Local, nLocal);
if (nextSepN >= totalRadius && prevSepN < totalRadius)
{
exitStartIndex = (i == 0) ? vertexCount-1 : i-1;
exitEndIndex = i;
exitSepN = prevSepN;
}
else if (nextSepN < totalRadius && prevSepN >= totalRadius)
{
enterStartIndex = (i == 0) ? vertexCount-1 : i-1;
enterEndIndex = i;
enterSepN = nextSepN;
}
if (nextSepN < deepestSepN)
{
deepestSepN = nextSepN;
//deepestIndex = i;
}
prevSepN = nextSepN;
}
if (enterStartIndex == -1)
{
// Edge normal separation
// polygon is entirely below or entirely above edge, return with no contact:
return;
}
if (separationMax > totalRadius)
{
// Face normal separation
// polygon is laterally disjoint with edge, return with no contact:
return;
}
// if the polygon is near a convex corner on the edge
if ((separationV1 && edge->Corner1IsConvex()) || (!separationV1 && edge->Corner2IsConvex()))
{
// if shallowest depth was from a polygon normal (meaning the polygon face is longer than the edge shape),
// use the edge's vertex as the contact point:
if (separationMax > deepestSepN + b2_linearSlop)
{
// if -normal angle is closer to adjacent edge than this edge,
// let the adjacent edge handle it and return with no contact:
if (separationV1)
{
if (b2Dot(normals[separationIndex1], b2MulT(transformA.R, b2Mul(transformB.R, edge->GetCorner1Vector()))) >= 0.0f)
{
return;
}
}
else
{
if (b2Dot(normals[separationIndex2], b2MulT(transformA.R, b2Mul(transformB.R, edge->GetCorner2Vector()))) <= 0.0f)
{
return;
}
}
manifold->m_pointCount = 1;
manifold->m_type = b2Manifold::e_faceA
manifold->m_localPlaneNormal = normals[separationIndex];
manifold->m_points[0].m_id.key = 0;
manifold->m_points[0].m_id.features.incidentEdge = (uint8)separationIndex;
manifold->m_points[0].m_id.features.incidentVertex = b2_nullFeature;
manifold->m_points[0].m_id.features.referenceEdge = 0;
manifold->m_points[0].m_id.features.flip = 0;
if (separationV1)
{
manifold->m_points[0].m_localPoint = edge->GetVertex1();
}
else
{
manifold->m_points[0].m_localPoint = edge->GetVertex2();
}
return;
}
}
// We're going to use the edge's normal now.
manifold->m_localPlaneNormal = edge->GetNormalVector();
manifold->m_localPoint = 0.5f * (edge->m_v1 + edge->m_v2);
// Check whether we only need one contact point.
if (enterEndIndex == exitStartIndex)
{
manifold->m_pointCount = 1;
manifold->m_points[0].m_id.key = 0;
manifold->m_points[0].m_id.features.incidentEdge = (uint8)enterEndIndex;
manifold->m_points[0].m_id.features.incidentVertex = b2_nullFeature;
manifold->m_points[0].m_id.features.referenceEdge = 0;
manifold->m_points[0].m_id.features.flip = 0;
manifold->m_points[0].m_localPoint = vertices[enterEndIndex];
return;
}
manifold->m_pointCount = 2;
// dirLocal should be the edge's direction vector, but in the frame of the polygon.
b2Vec2 dirLocal = b2Cross(nLocal, -1.0f); // TODO: figure out why this optimization didn't work
//b2Vec2 dirLocal = b2MulT(transformA.R, b2Mul(transformB.R, edge->GetDirectionVector()));
float32 dirProj1 = b2Dot(dirLocal, vertices[enterEndIndex] - v1Local);
float32 dirProj2;
// The contact resolution is more robust if the two manifold points are
// adjacent to each other on the polygon. So pick the first two polygon
// vertices that are under the edge:
exitEndIndex = (enterEndIndex == vertexCount - 1) ? 0 : enterEndIndex + 1;
if (exitEndIndex != exitStartIndex)
{
exitStartIndex = exitEndIndex;
exitSepN = b2Dot(nLocal, vertices[exitStartIndex] - v1Local);
}
dirProj2 = b2Dot(dirLocal, vertices[exitStartIndex] - v1Local);
manifold->m_points[0].m_id.key = 0;
manifold->m_points[0].m_id.features.incidentEdge = (uint8)enterEndIndex;
manifold->m_points[0].m_id.features.incidentVertex = b2_nullFeature;
manifold->m_points[0].m_id.features.referenceEdge = 0;
manifold->m_points[0].m_id.features.flip = 0;
if (dirProj1 > edge->GetLength())
{
manifold->m_points[0].localPointA = v2Local;
manifold->m_points[0].localPointB = edge->GetVertex2();
}
else
{
manifold->m_points[0].localPointA = vertices[enterEndIndex];
manifold->m_points[0].localPointB = b2MulT(transformB, b2Mul(transformA, vertices[enterEndIndex]));
}
manifold->m_points[1].m_id.key = 0;
manifold->m_points[1].m_id.features.incidentEdge = (uint8)exitStartIndex;
manifold->m_points[1].m_id.features.incidentVertex = b2_nullFeature;
manifold->m_points[1].m_id.features.referenceEdge = 0;
manifold->m_points[1].m_id.features.flip = 0;
if (dirProj2 < 0.0f)
{
manifold->m_points[1].localPointA = v1Local;
manifold->m_points[1].localPointB = edge->GetVertex1();
}
else
{
manifold->m_points[1].localPointA = vertices[exitStartIndex];
manifold->m_points[1].localPointB = b2MulT(transformB, b2Mul(transformA, vertices[exitStartIndex]));
manifold->m_points[1].separation = exitSepN - totalRadius;
}
}
void b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const
{
float32 lower = 0.0f, upper = input.maxFraction;
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
b2Vec2 d = p2 - p1;
int32 index = -1;
output->hit = false;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
if (upper < lower)
{
return;
}
}
b2Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output->hit = true;
output->fraction = lower;
output->normal = b2Mul(xf.R, m_normals[index]);
return;
}
}
// This implements 2-sided edge vs circle collision.
void b2CollideEdgeAndCircle(b2Manifold* manifold,
const b2EdgeShape* edge,
const b2XForm& transformA,
const b2CircleShape* circle,
const b2XForm& transformB)
{
manifold->m_pointCount = 0;
b2Vec2 cLocal = b2MulT(transformA, b2Mul(transformB, circle->m_p));
b2Vec2 normal = edge->m_normal;
b2Vec2 v1 = edge->m_v1;
b2Vec2 v2 = edge->m_v2;
float32 radius = edge->m_radius + circle->m_radius;
// Barycentric coordinates
float32 u1 = b2Dot(cLocal - v1, v2 - v1);
float32 u2 = b2Dot(cLocal - v2, v1 - v2);
if (u1 <= 0.0f)
{
// Behind v1
if (b2DistanceSquared(cLocal, v1) > radius * radius)
{
return;
}
manifold->m_pointCount = 1;
manifold->m_type = b2Manifold::e_faceA;
manifold->m_localPlaneNormal = cLocal - v1;
manifold->m_localPlaneNormal.Normalize();
manifold->m_localPoint = v1;
manifold->m_points[0].m_localPoint = circle->m_p;
manifold->m_points[0].m_id.key = 0;
}
else if (u2 <= 0.0f)
{
// Ahead of v2
if (b2DistanceSquared(cLocal, v2) > radius * radius)
{
return;
}
manifold->m_pointCount = 1;
manifold->m_type = b2Manifold::e_faceA;
manifold->m_localPlaneNormal = cLocal - v2;
manifold->m_localPlaneNormal.Normalize();
manifold->m_localPoint = v2;
manifold->m_points[0].m_localPoint = circle->m_p;
manifold->m_points[0].m_id.key = 0;
}
else
{
float32 separation = b2Dot(cLocal - v1, normal);
if (separation < -radius || radius < separation)
{
return;
}
manifold->m_pointCount = 1;
manifold->m_type = b2Manifold::e_faceA;
manifold->m_localPlaneNormal = separation < 0.0f ? -normal : normal;
manifold->m_localPoint = 0.5f * (v1 + v2);
manifold->m_points[0].m_localPoint = circle->m_p;
manifold->m_points[0].m_id.key = 0;
}
}
// The normal points from 1 to 2
void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polyA, const b2Transform& xfA,
const b2PolygonShape* polyB, const b2Transform& xfB)
{
manifold->m_pointCount = 0;
float32 totalRadius = polyA->m_radius + polyB->m_radius;
int32 edgeA = 0;
float32 separationA = b2FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > totalRadius)
return;
int32 edgeB = 0;
float32 separationB = b2FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
if (separationB > totalRadius)
return;
const b2PolygonShape* poly1; // reference polygon
const b2PolygonShape* poly2; // incident polygon
b2Transform xf1, xf2;
int32 edge1; // reference edge
uint8 flip;
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
manifold->m_type = b2Manifold::e_faceB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
manifold->m_type = b2Manifold::e_faceA;
flip = 0;
}
b2ClipVertex incidentEdge[2];
b2FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int32 count1 = poly1->m_vertexCount;
const b2Vec2* vertices1 = poly1->m_vertices;
b2Vec2 v11 = vertices1[edge1];
b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
b2Vec2 localTangent = v12 - v11;
localTangent.Normalize();
b2Vec2 localNormal = b2Cross(localTangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
b2Vec2 tangent = b2Mul(xf1.R, localTangent);
b2Vec2 normal = b2Cross(tangent, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold->m_localPlaneNormal = localNormal;
manifold->m_localPoint = planePoint;
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint* cp = manifold->m_points + pointCount;
cp->m_localPoint = b2MulT(xf2, clipPoints2[i].v);
cp->m_id = clipPoints2[i].id;
cp->m_id.features.flip = flip;
++pointCount;
}
}
manifold->m_pointCount = pointCount;
}
bool b2GearJoint::SolvePositionConstraints(const b2SolverData& data)
{
b2Vec2 cA = data.positions[m_indexA].c;
float aA = data.positions[m_indexA].a;
b2Vec2 cB = data.positions[m_indexB].c;
float aB = data.positions[m_indexB].a;
b2Vec2 cC = data.positions[m_indexC].c;
float aC = data.positions[m_indexC].a;
b2Vec2 cD = data.positions[m_indexD].c;
float aD = data.positions[m_indexD].a;
b2Rot qA(aA), qB(aB), qC(aC), qD(aD);
float linearError = 0.0f;
float coordinateA, coordinateB;
b2Vec2 JvAC, JvBD;
float JwA, JwB, JwC, JwD;
float mass = 0.0f;
if (m_typeA == e_revoluteJoint)
{
JvAC.SetZero();
JwA = 1.0f;
JwC = 1.0f;
mass += m_iA + m_iC;
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2Vec2 u = b2Mul(qC, m_localAxisC);
b2Vec2 rC = b2Mul(qC, m_localAnchorC - m_lcC);
b2Vec2 rA = b2Mul(qA, m_localAnchorA - m_lcA);
JvAC = u;
JwC = b2Cross(rC, u);
JwA = b2Cross(rA, u);
mass += m_mC + m_mA + m_iC * JwC * JwC + m_iA * JwA * JwA;
b2Vec2 pC = m_localAnchorC - m_lcC;
b2Vec2 pA = b2MulT(qC, rA + (cA - cC));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
if (m_typeB == e_revoluteJoint)
{
JvBD.SetZero();
JwB = m_ratio;
JwD = m_ratio;
mass += m_ratio * m_ratio * (m_iB + m_iD);
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2Vec2 u = b2Mul(qD, m_localAxisD);
b2Vec2 rD = b2Mul(qD, m_localAnchorD - m_lcD);
b2Vec2 rB = b2Mul(qB, m_localAnchorB - m_lcB);
JvBD = m_ratio * u;
JwD = m_ratio * b2Cross(rD, u);
JwB = m_ratio * b2Cross(rB, u);
mass += m_ratio * m_ratio * (m_mD + m_mB) + m_iD * JwD * JwD + m_iB * JwB * JwB;
b2Vec2 pD = m_localAnchorD - m_lcD;
b2Vec2 pB = b2MulT(qD, rB + (cB - cD));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
float C = (coordinateA + m_ratio * coordinateB) - m_constant;
float impulse = 0.0f;
if (mass > 0.0f)
{
impulse = -C / mass;
}
cA += m_mA * impulse * JvAC;
aA += m_iA * impulse * JwA;
cB += m_mB * impulse * JvBD;
aB += m_iB * impulse * JwB;
cC -= m_mC * impulse * JvAC;
aC -= m_iC * impulse * JwC;
cD -= m_mD * impulse * JvBD;
aD -= m_iD * impulse * JwD;
data.positions[m_indexA].c = cA;
data.positions[m_indexA].a = aA;
data.positions[m_indexB].c = cB;
data.positions[m_indexB].a = aB;
data.positions[m_indexC].c = cC;
data.positions[m_indexC].a = aC;
data.positions[m_indexD].c = cD;
data.positions[m_indexD].a = aD;
// TODO_ERIN not implemented
return linearError < b2_linearSlop;
}
// The normal points from 1 to 2
void b2CollidePolygons(b2Manifold* manifold,
const b2PolygonShape* polyA, const b2XForm& xfA,
const b2PolygonShape* polyB, const b2XForm& xfB)
{
manifold->pointCount = 0;
int32 edgeA = 0;
float32 separationA = FindMaxSeparation(&edgeA, polyA, xfA, polyB, xfB);
if (separationA > 0.0f)
return;
int32 edgeB = 0;
float32 separationB = FindMaxSeparation(&edgeB, polyB, xfB, polyA, xfA);
if (separationB > 0.0f)
return;
const b2PolygonShape* poly1; // reference poly
const b2PolygonShape* poly2; // incident poly
b2XForm xf1, xf2;
int32 edge1; // reference edge
uint8 flip;
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
// TODO_ERIN use "radius" of poly for absolute tolerance.
if (separationB > k_relativeTol * separationA + k_absoluteTol)
{
poly1 = polyB;
poly2 = polyA;
xf1 = xfB;
xf2 = xfA;
edge1 = edgeB;
flip = 1;
}
else
{
poly1 = polyA;
poly2 = polyB;
xf1 = xfA;
xf2 = xfB;
edge1 = edgeA;
flip = 0;
}
ClipVertex incidentEdge[2];
FindIncidentEdge(incidentEdge, poly1, xf1, edge1, poly2, xf2);
int32 count1 = poly1->GetVertexCount();
const b2Vec2* vertices1 = poly1->GetVertices();
b2Vec2 v11 = vertices1[edge1];
b2Vec2 v12 = edge1 + 1 < count1 ? vertices1[edge1+1] : vertices1[0];
b2Vec2 dv = v12 - v11;
b2Vec2 sideNormal = b2Mul(xf1.R, v12 - v11);
sideNormal.Normalize();
b2Vec2 frontNormal = b2Cross(sideNormal, 1.0f);
v11 = b2Mul(xf1, v11);
v12 = b2Mul(xf1, v12);
float32 frontOffset = b2Dot(frontNormal, v11);
float32 sideOffset1 = -b2Dot(sideNormal, v11);
float32 sideOffset2 = b2Dot(sideNormal, v12);
// Clip incident edge against extruded edge1 side edges.
ClipVertex clipPoints1[2];
ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = ClipSegmentToLine(clipPoints1, incidentEdge, -sideNormal, sideOffset1);
if (np < 2)
return;
// Clip to negative box side 1
np = ClipSegmentToLine(clipPoints2, clipPoints1, sideNormal, sideOffset2);
if (np < 2)
{
return;
}
// Now clipPoints2 contains the clipped points.
manifold->normal = flip ? -frontNormal : frontNormal;
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation = b2Dot(frontNormal, clipPoints2[i].v) - frontOffset;
if (separation <= 0.0f)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
cp->separation = separation;
cp->localPoint1 = b2MulT(xfA, clipPoints2[i].v);
cp->localPoint2 = b2MulT(xfB, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
cp->id.features.flip = flip;
++pointCount;
}
}
manifold->pointCount = pointCount;}
void b2BuoyancyController::Step(const b2TimeStep& step)
{
B2_NOT_USED(step);
if(!m_bodyList)
return;
if(useWorldGravity)
{
gravity = m_world->GetGravity();
}
for(b2ControllerEdge *i=m_bodyList;i;i=i->nextBody)
{
b2Body* body = i->body;
if(body->IsSleeping())
{
//Buoyancy force is just a function of position,
//so unlike most forces, it is safe to ignore sleeping bodes
continue;
}
b2Vec2 areac(0,0);
b2Vec2 massc(0,0);
float32 area = 0;
float32 mass = 0;
for(b2Fixture* shape=body->GetFixtureList();shape;shape=shape->GetNext())
{
b2Vec2 sc(0,0);
float32 sarea = shape->ComputeSubmergedArea(normal, offset, &sc);
area += sarea;
areac.x += sarea * sc.x;
areac.y += sarea * sc.y;
float shapeDensity = 0;
if(useDensity)
{
//TODO: Expose density publicly
shapeDensity=shape->GetDensity();
}
else
{
shapeDensity = 1;
}
mass += sarea*shapeDensity;
massc.x += sarea * sc.x * shapeDensity;
massc.y += sarea * sc.y * shapeDensity;
}
areac.x/=area;
areac.y/=area;
b2Vec2 localCentroid = b2MulT(body->GetXForm(),areac);
massc.x/=mass;
massc.y/=mass;
if(area<B2_FLT_EPSILON)
continue;
//Buoyancy
b2Vec2 buoyancyForce = -density*area*gravity;
body->ApplyForce(buoyancyForce,massc);
//Linear drag
b2Vec2 dragForce = body->GetLinearVelocityFromWorldPoint(areac) - velocity;
dragForce *= -linearDrag*area;
body->ApplyForce(dragForce,areac);
//Angular drag
//TODO: Something that makes more physical sense?
body->ApplyTorque(-body->GetInertia()/body->GetMass()*area*body->GetAngularVelocity()*angularDrag);
}
}
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
float32 lower = 0.0f, upper = input.maxFraction;
int32 index = -1;
for (int32 i = 0; i < m_count; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return false;
}
}
b2Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output->fraction = lower;
output->normal = b2Mul(xf.q, m_normals[index]);
return true;
}
return false;
}
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1
bool b2EdgeShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input,
const b2Transform& xf, int32 childIndex) const
{
B2_NOT_USED(childIndex);
// Put the ray into the edge's frame of reference.
b2Vec2 p1 = b2MulT(xf.q, input.p1 - xf.p);
b2Vec2 p2 = b2MulT(xf.q, input.p2 - xf.p);
b2Vec2 d = p2 - p1;
b2Vec2 v1 = m_vertex1;
b2Vec2 v2 = m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 normal(e.y, -e.x);
normal.Normalize();
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float32 numerator = b2Dot(normal, v1 - p1);
float32 denominator = b2Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float32 t = numerator / denominator;
if (t < 0.0f || input.maxFraction < t)
{
return false;
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float32 rr = b2Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float32 s = b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output->fraction = t;
if (numerator > 0.0f)
{
output->normal = -b2Mul(xf.q, normal);
}
else
{
output->normal = b2Mul(xf.q, normal);
}
return true;
}
void b2Distance(b2DistanceOutput* output,
b2SimplexCache* cache,
const b2DistanceInput* input)
{
++b2_gjkCalls;
const b2DistanceProxy* proxyA = &input->proxyA;
const b2DistanceProxy* proxyB = &input->proxyB;
b2Transform transformA = input->transformA;
b2Transform transformB = input->transformB;
// Initialize the simplex.
b2Simplex simplex;
simplex.ReadCache(cache, proxyA, transformA, proxyB, transformB);
// Get simplex vertices as an array.
b2SimplexVertex* vertices = &simplex.m_v1;
const int32 k_maxIters = 20;
// These store the vertices of the last simplex so that we
// can check for duplicates and prevent cycling.
int32 saveA[3], saveB[3];
int32 saveCount = 0;
b2Vec2 closestPoint = simplex.GetClosestPoint();
float32 distanceSqr1 = closestPoint.LengthSquared();
float32 distanceSqr2 = distanceSqr1;
// Main iteration loop.
int32 iter = 0;
while (iter < k_maxIters)
{
// Copy simplex so we can identify duplicates.
saveCount = simplex.m_count;
for (int32 i = 0; i < saveCount; ++i)
{
saveA[i] = vertices[i].indexA;
saveB[i] = vertices[i].indexB;
}
switch (simplex.m_count)
{
case 1:
break;
case 2:
simplex.Solve2();
break;
case 3:
simplex.Solve3();
break;
default:
b2Assert(false);
}
// If we have 3 points, then the origin is in the corresponding triangle.
if (simplex.m_count == 3)
{
break;
}
// Compute closest point.
b2Vec2 p = simplex.GetClosestPoint();
distanceSqr2 = p.LengthSquared();
// Ensure progress
if (distanceSqr2 >= distanceSqr1)
{
//break;
}
distanceSqr1 = distanceSqr2;
// Get search direction.
b2Vec2 d = simplex.GetSearchDirection();
// Ensure the search direction is numerically fit.
if (d.LengthSquared() < b2_epsilon * b2_epsilon)
{
// The origin is probably contained by a line segment
// or triangle. Thus the shapes are overlapped.
// We can't return zero here even though there may be overlap.
// In case the simplex is a point, segment, or triangle it is difficult
// to determine if the origin is contained in the CSO or very close to it.
break;
}
// Compute a tentative new simplex vertex using support points.
b2SimplexVertex* vertex = vertices + simplex.m_count;
vertex->indexA = proxyA->GetSupport(b2MulT(transformA.q, -d));
vertex->wA = b2Mul(transformA, proxyA->GetVertex(vertex->indexA));
b2Vec2 wBLocal;
vertex->indexB = proxyB->GetSupport(b2MulT(transformB.q, d));
vertex->wB = b2Mul(transformB, proxyB->GetVertex(vertex->indexB));
vertex->w = vertex->wB - vertex->wA;
// Iteration count is equated to the number of support point calls.
++iter;
++b2_gjkIters;
// Check for duplicate support points. This is the main termination criteria.
bool duplicate = false;
for (int32 i = 0; i < saveCount; ++i)
{
if (vertex->indexA == saveA[i] && vertex->indexB == saveB[i])
{
duplicate = true;
break;
}
}
// If we found a duplicate support point we must exit to avoid cycling.
if (duplicate)
{
break;
}
// New vertex is ok and needed.
++simplex.m_count;
}
b2_gjkMaxIters = b2Max(b2_gjkMaxIters, iter);
// Prepare output.
simplex.GetWitnessPoints(&output->pointA, &output->pointB);
output->distance = b2Distance(output->pointA, output->pointB);
output->iterations = iter;
// Cache the simplex.
simplex.WriteCache(cache);
// Apply radii if requested.
if (input->useRadii)
{
float32 rA = proxyA->m_radius;
float32 rB = proxyB->m_radius;
if (output->distance > rA + rB && output->distance > b2_epsilon)
{
// Shapes are still no overlapped.
// Move the witness points to the outer surface.
output->distance -= rA + rB;
b2Vec2 normal = output->pointB - output->pointA;
normal.Normalize();
output->pointA += rA * normal;
output->pointB -= rB * normal;
}
else
{
// Shapes are overlapped when radii are considered.
// Move the witness points to the middle.
b2Vec2 p = 0.5f * (output->pointA + output->pointB);
output->pointA = p;
output->pointB = p;
output->distance = 0.0f;
}
}
}
bool b2PolygonShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& xf) const
{
// Put the ray into the polygon's frame of reference.
b2Vec2 p1 = b2MulT(xf.R, input.p1 - xf.position);
b2Vec2 p2 = b2MulT(xf.R, input.p2 - xf.position);
b2Vec2 d = p2 - p1;
if (m_vertexCount == 2)
{
b2Vec2 v1 = m_vertices[0];
b2Vec2 v2 = m_vertices[1];
b2Vec2 normal = m_normals[0];
// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
float32 numerator = b2Dot(normal, v1 - p1);
float32 denominator = b2Dot(normal, d);
if (denominator == 0.0f)
{
return false;
}
float32 t = numerator / denominator;
if (t < 0.0f || 1.0f < t)
{
return false;
}
b2Vec2 q = p1 + t * d;
// q = v1 + s * r
// s = dot(q - v1, r) / dot(r, r)
b2Vec2 r = v2 - v1;
float32 rr = b2Dot(r, r);
if (rr == 0.0f)
{
return false;
}
float32 s = b2Dot(q - v1, r) / rr;
if (s < 0.0f || 1.0f < s)
{
return false;
}
output->fraction = t;
if (numerator > 0.0f)
{
output->normal = -normal;
}
else
{
output->normal = normal;
}
return true;
}
else
{
float32 lower = 0.0f, upper = input.maxFraction;
int32 index = -1;
for (int32 i = 0; i < m_vertexCount; ++i)
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
float32 numerator = b2Dot(m_normals[i], m_vertices[i] - p1);
float32 denominator = b2Dot(m_normals[i], d);
if (denominator == 0.0f)
{
if (numerator < 0.0f)
{
return false;
}
}
else
{
// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < 0.0f && numerator < lower * denominator)
{
// Increase lower.
// The segment enters this half-space.
lower = numerator / denominator;
index = i;
}
else if (denominator > 0.0f && numerator < upper * denominator)
{
// Decrease upper.
// The segment exits this half-space.
upper = numerator / denominator;
}
}
// The use of epsilon here causes the assert on lower to trip
// in some cases. Apparently the use of epsilon was to make edge
// shapes work, but now those are handled separately.
//if (upper < lower - b2_epsilon)
if (upper < lower)
{
return false;
}
}
b2Assert(0.0f <= lower && lower <= input.maxFraction);
if (index >= 0)
{
output->fraction = lower;
output->normal = b2Mul(xf.R, m_normals[index]);
return true;
}
}
return false;
}
b2GearJoint::b2GearJoint(const b2GearJointDef* def)
: b2Joint(def)
{
m_joint1 = def->joint1;
m_joint2 = def->joint2;
m_typeA = m_joint1->GetType();
m_typeB = m_joint2->GetType();
b2Assert(m_typeA == e_revoluteJoint || m_typeA == e_prismaticJoint);
b2Assert(m_typeB == e_revoluteJoint || m_typeB == e_prismaticJoint);
float coordinateA, coordinateB;
// TODO_ERIN there might be some problem with the joint edges in b2Joint.
m_bodyC = m_joint1->GetBodyA();
m_bodyA = m_joint1->GetBodyB();
// Get geometry of joint1
b2Transform xfA = m_bodyA->m_xf;
float aA = m_bodyA->m_sweep.a;
b2Transform xfC = m_bodyC->m_xf;
float aC = m_bodyC->m_sweep.a;
if (m_typeA == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint1;
m_localAnchorC = revolute->m_localAnchorA;
m_localAnchorA = revolute->m_localAnchorB;
m_referenceAngleA = revolute->m_referenceAngle;
m_localAxisC.SetZero();
coordinateA = aA - aC - m_referenceAngleA;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint1;
m_localAnchorC = prismatic->m_localAnchorA;
m_localAnchorA = prismatic->m_localAnchorB;
m_referenceAngleA = prismatic->m_referenceAngle;
m_localAxisC = prismatic->m_localXAxisA;
b2Vec2 pC = m_localAnchorC;
b2Vec2 pA = b2MulT(xfC.q, b2Mul(xfA.q, m_localAnchorA) + (xfA.p - xfC.p));
coordinateA = b2Dot(pA - pC, m_localAxisC);
}
m_bodyD = m_joint2->GetBodyA();
m_bodyB = m_joint2->GetBodyB();
// Get geometry of joint2
b2Transform xfB = m_bodyB->m_xf;
float aB = m_bodyB->m_sweep.a;
b2Transform xfD = m_bodyD->m_xf;
float aD = m_bodyD->m_sweep.a;
if (m_typeB == e_revoluteJoint)
{
b2RevoluteJoint* revolute = (b2RevoluteJoint*)def->joint2;
m_localAnchorD = revolute->m_localAnchorA;
m_localAnchorB = revolute->m_localAnchorB;
m_referenceAngleB = revolute->m_referenceAngle;
m_localAxisD.SetZero();
coordinateB = aB - aD - m_referenceAngleB;
}
else
{
b2PrismaticJoint* prismatic = (b2PrismaticJoint*)def->joint2;
m_localAnchorD = prismatic->m_localAnchorA;
m_localAnchorB = prismatic->m_localAnchorB;
m_referenceAngleB = prismatic->m_referenceAngle;
m_localAxisD = prismatic->m_localXAxisA;
b2Vec2 pD = m_localAnchorD;
b2Vec2 pB = b2MulT(xfD.q, b2Mul(xfB.q, m_localAnchorB) + (xfB.p - xfD.p));
coordinateB = b2Dot(pB - pD, m_localAxisD);
}
m_ratio = def->ratio;
m_constant = coordinateA + m_ratio * coordinateB;
m_impulse = 0.0f;
}
// Collide and edge and polygon. This uses the SAT and clipping to produce up to 2 contact points.
// Edge adjacency is handle to produce locally valid contact points and normals. This is intended
// to allow the polygon to slide smoothly over an edge chain.
//
// Algorithm
// 1. Classify front-side or back-side collision with edge.
// 2. Compute separation
// 3. Process adjacent edges
// 4. Classify adjacent edge as convex, flat, null, or concave
// 5. Skip null or concave edges. Concave edges get a separate manifold.
// 6. If the edge is flat, compute contact points as normal. Discard boundary points.
// 7. If the edge is convex, compute it's separation.
// 8. Use the minimum separation of up to three edges. If the minimum separation
// is not the primary edge, return.
// 9. If the minimum separation is the primary edge, compute the contact points and return.
void b2CollideEdgeAndPolygon( b2Manifold* manifold,
const b2EdgeShape* edgeA, const b2Transform& xfA,
const b2PolygonShape* polygonB_in, const b2Transform& xfB)
{
manifold->pointCount = 0;
b2Transform xf = b2MulT(xfA, xfB);
// Create a polygon for edge shape A
b2PolygonShape polygonA;
polygonA.SetAsEdge(edgeA->m_vertex1, edgeA->m_vertex2);
// Build polygonB in frame A
b2PolygonShape polygonB;
polygonB.m_radius = polygonB_in->m_radius;
polygonB.m_vertexCount = polygonB_in->m_vertexCount;
polygonB.m_centroid = b2Mul(xf, polygonB_in->m_centroid);
for (int32 i = 0; i < polygonB.m_vertexCount; ++i)
{
polygonB.m_vertices[i] = b2Mul(xf, polygonB_in->m_vertices[i]);
polygonB.m_normals[i] = b2Mul(xf.R, polygonB_in->m_normals[i]);
}
float32 totalRadius = polygonA.m_radius + polygonB.m_radius;
// Edge geometry
b2Vec2 v1 = edgeA->m_vertex1;
b2Vec2 v2 = edgeA->m_vertex2;
b2Vec2 e = v2 - v1;
b2Vec2 edgeNormal(e.y, -e.x);
edgeNormal.Normalize();
// Determine side
bool isFrontSide = b2Dot(edgeNormal, polygonB.m_centroid - v1) >= 0.0f;
if (isFrontSide == false)
{
edgeNormal = -edgeNormal;
}
// Compute primary separating axis
b2EPAxis edgeAxis = b2EPEdgeSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
if (edgeAxis.separation > totalRadius)
{
// Shapes are separated
return;
}
// Classify adjacent edges
b2EdgeType types[2] = {b2_isolated, b2_isolated};
if (edgeA->m_hasVertex0)
{
b2Vec2 v0 = edgeA->m_vertex0;
float32 s = b2Dot(edgeNormal, v0 - v1);
if (s > 0.1f * b2_linearSlop)
{
types[0] = b2_concave;
}
else if (s >= -0.1f * b2_linearSlop)
{
types[0] = b2_flat;
}
else
{
types[0] = b2_convex;
}
}
if (edgeA->m_hasVertex3)
{
b2Vec2 v3 = edgeA->m_vertex3;
float32 s = b2Dot(edgeNormal, v3 - v2);
if (s > 0.1f * b2_linearSlop)
{
types[1] = b2_concave;
}
else if (s >= -0.1f * b2_linearSlop)
{
types[1] = b2_flat;
}
else
{
types[1] = b2_convex;
}
}
if (types[0] == b2_convex)
{
// Check separation on previous edge.
b2Vec2 v0 = edgeA->m_vertex0;
b2Vec2 e0 = v1 - v0;
b2Vec2 n0(e0.y, -e0.x);
n0.Normalize();
if (isFrontSide == false)
{
n0 = -n0;
}
b2EPAxis axis1 = b2EPEdgeSeparation(v0, v1, n0, &polygonB, totalRadius);
if (axis1.separation > edgeAxis.separation)
{
// The polygon should collide with previous edge
return;
}
}
if (types[1] == b2_convex)
{
// Check separation on next edge.
b2Vec2 v3 = edgeA->m_vertex3;
b2Vec2 e2 = v3 - v2;
b2Vec2 n2(e2.y, -e2.x);
n2.Normalize();
if (isFrontSide == false)
{
n2 = -n2;
}
b2EPAxis axis2 = b2EPEdgeSeparation(v2, v3, n2, &polygonB, totalRadius);
if (axis2.separation > edgeAxis.separation)
{
// The polygon should collide with the next edge
return;
}
}
b2EPAxis polygonAxis = b2EPPolygonSeparation(v1, v2, edgeNormal, &polygonB, totalRadius);
if (polygonAxis.separation > totalRadius)
{
return;
}
// Use hysteresis for jitter reduction.
const float32 k_relativeTol = 0.98f;
const float32 k_absoluteTol = 0.001f;
b2EPAxis primaryAxis;
if (polygonAxis.separation > k_relativeTol * edgeAxis.separation + k_absoluteTol)
{
primaryAxis = polygonAxis;
}
else
{
primaryAxis = edgeAxis;
}
b2PolygonShape* poly1;
b2PolygonShape* poly2;
b2ClipVertex incidentEdge[2];
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
poly1 = &polygonA;
poly2 = &polygonB;
if (isFrontSide == false)
{
primaryAxis.index = 1;
}
manifold->type = b2Manifold::e_faceA;
}
else
{
poly1 = &polygonB;
poly2 = &polygonA;
manifold->type = b2Manifold::e_faceB;
}
int32 edge1 = primaryAxis.index;
b2FindIncidentEdge(incidentEdge, poly1, primaryAxis.index, poly2);
int32 count1 = poly1->m_vertexCount;
const b2Vec2* vertices1 = poly1->m_vertices;
int32 iv1 = edge1;
int32 iv2 = edge1 + 1 < count1 ? edge1 + 1 : 0;
b2Vec2 v11 = vertices1[iv1];
b2Vec2 v12 = vertices1[iv2];
b2Vec2 tangent = v12 - v11;
tangent.Normalize();
b2Vec2 normal = b2Cross(tangent, 1.0f);
b2Vec2 planePoint = 0.5f * (v11 + v12);
// Face offset.
float32 frontOffset = b2Dot(normal, v11);
// Side offsets, extended by polytope skin thickness.
float32 sideOffset1 = -b2Dot(tangent, v11) + totalRadius;
float32 sideOffset2 = b2Dot(tangent, v12) + totalRadius;
// Clip incident edge against extruded edge1 side edges.
b2ClipVertex clipPoints1[2];
b2ClipVertex clipPoints2[2];
int np;
// Clip to box side 1
np = b2ClipSegmentToLine(clipPoints1, incidentEdge, -tangent, sideOffset1, iv1);
if (np < b2_maxManifoldPoints)
{
return;
}
// Clip to negative box side 1
np = b2ClipSegmentToLine(clipPoints2, clipPoints1, tangent, sideOffset2, iv2);
if (np < b2_maxManifoldPoints)
{
return;
}
// Now clipPoints2 contains the clipped points.
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
manifold->localNormal = normal;
manifold->localPoint = planePoint;
}
else
{
manifold->localNormal = b2MulT(xf.R, normal);
manifold->localPoint = b2MulT(xf, planePoint);
}
int32 pointCount = 0;
for (int32 i = 0; i < b2_maxManifoldPoints; ++i)
{
float32 separation;
separation = b2Dot(normal, clipPoints2[i].v) - frontOffset;
if (separation <= totalRadius)
{
b2ManifoldPoint* cp = manifold->points + pointCount;
if (primaryAxis.type == b2EPAxis::e_edgeA)
{
cp->localPoint = b2MulT(xf, clipPoints2[i].v);
cp->id = clipPoints2[i].id;
}
else
{
cp->localPoint = clipPoints2[i].v;
cp->id.cf.typeA = clipPoints2[i].id.cf.typeB;
cp->id.cf.typeB = clipPoints2[i].id.cf.typeA;
cp->id.cf.indexA = clipPoints2[i].id.cf.indexB;
cp->id.cf.indexB = clipPoints2[i].id.cf.indexA;
}
if (cp->id.cf.typeA == b2ContactFeature::e_vertex && types[cp->id.cf.indexA] == b2_flat)
{
continue;
}
++pointCount;
}
}
manifold->pointCount = pointCount;
}
