float32 b2CircleShape::ComputeSubmergedArea( const b2Vec2& normal,
float32 offset,
const b2XForm& xf,
b2Vec2* c) const
{
b2Vec2 p = b2Mul(xf,m_localPosition);
float32 l = -(b2Dot(normal,p) - offset);
if(l<-m_radius+B2_FLT_EPSILON){
//Completely dry
return 0;
}
if(l>m_radius){
//Completely wet
*c = p;
return b2_pi*m_radius*m_radius;
}
//Magic
float32 r2 = m_radius*m_radius;
float32 l2 = l*l;
//TODO: write b2Sqrt to handle fixed point case.
float32 area = r2 * (asin(l/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;
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
bool b2CircleShape::RayCast(b2RayCastOutput* output, const b2RayCastInput& input, const b2Transform& transform) const
{
b2Vec2 position = transform.position + b2Mul(transform.R, m_p);
b2Vec2 s = input.p1 - position;
float32 b = b2Dot(s, s) - m_radius * m_radius;
// Solve quadratic equation.
b2Vec2 r = input.p2 - input.p1;
float32 c = b2Dot(s, r);
float32 rr = b2Dot(r, r);
float32 sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < b2_epsilon)
{
return false;
}
// Find the point of intersection of the line with the circle.
float32 a = -(c + b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= input.maxFraction * rr)
{
a /= rr;
output->fraction = a;
output->normal = s + a * r;
output->normal.Normalize();
return true;
}
return false;
}
void b2EdgeShape::UpdateSweepRadius(const b2Vec2& center)
{
// Update the sweep radius (maximum radius) as measured from
// a local center point.
b2Vec2 d = m_coreV1 - center;
float32 d1 = b2Dot(d,d);
d = m_coreV2 - center;
float32 d2 = b2Dot(d,d);
m_sweepRadius = b2Sqrt(d1 > d2 ? d1 : d2);
}
int32 b2CalculateParticleIterations(
float32 gravity, float32 radius, float32 timeStep)
{
// In some situations you may want more particle iterations than this,
// but to avoid excessive cycle cost, don't recommend more than this.
const int32 B2_MAX_RECOMMENDED_PARTICLE_ITERATIONS = 8;
const float32 B2_RADIUS_THRESHOLD = 0.01f;
int32 iterations =
(int32) ceilf(b2Sqrt(gravity / (B2_RADIUS_THRESHOLD * radius)) * timeStep);
return b2Clamp(iterations, 1, B2_MAX_RECOMMENDED_PARTICLE_ITERATIONS);
}
void b2CollideCircles(
b2Manifold* manifold,
const b2CircleShape* circle1, const b2XForm& xf1,
const b2CircleShape* circle2, const b2XForm& xf2)
{
manifold->pointCount = 0;
b2Vec2 p1 = b2Mul(xf1, circle1->GetLocalPosition());
b2Vec2 p2 = b2Mul(xf2, circle2->GetLocalPosition());
b2Vec2 d = p2 - p1;
float32 distSqr = b2Dot(d, d);
float32 r1 = circle1->GetRadius();
float32 r2 = circle2->GetRadius();
float32 radiusSum = r1 + r2;
if (distSqr > radiusSum * radiusSum)
{
return;
}
float32 separation;
if (distSqr < B2_FLT_EPSILON)
{
separation = -radiusSum;
manifold->normal.Set(0.0f, 1.0f);
}
else
{
float32 dist = b2Sqrt(distSqr);
separation = dist - radiusSum;
float32 a = 1.0f / dist;
manifold->normal.x = a * d.x;
manifold->normal.y = a * d.y;
}
manifold->pointCount = 1;
manifold->points[0].id.key = 0;
manifold->points[0].separation = separation;
p1 += r1 * manifold->normal;
p2 -= r2 * manifold->normal;
b2Vec2 p = 0.5f * (p1 + p2);
manifold->points[0].localPoint1 = b2MulT(xf1, p);
manifold->points[0].localPoint2 = b2MulT(xf2, p);
}
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius
b2SegmentCollide b2CircleShape::TestSegment(const b2XForm& transform,
float32* lambda,
b2Vec2* normal,
const b2Segment& segment,
float32 maxLambda) const
{
b2Vec2 position = transform.position + b2Mul(transform.R, m_localPosition);
b2Vec2 s = segment.p1 - position;
float32 b = b2Dot(s, s) - m_radius * m_radius;
// Does the segment start inside the circle?
if (b < 0.0f)
{
*lambda = 0;
return e_startsInsideCollide;
}
// Solve quadratic equation.
b2Vec2 r = segment.p2 - segment.p1;
float32 c = b2Dot(s, r);
float32 rr = b2Dot(r, r);
float32 sigma = c * c - rr * b;
// Check for negative discriminant and short segment.
if (sigma < 0.0f || rr < B2_FLT_EPSILON)
{
return e_missCollide;
}
// Find the point of intersection of the line with the circle.
float32 a = -(c + b2Sqrt(sigma));
// Is the intersection point on the segment?
if (0.0f <= a && a <= maxLambda * rr)
{
a /= rr;
*lambda = a;
*normal = s + a * r;
normal->Normalize();
return e_hitCollide;
}
return e_missCollide;
}
void b2PolygonShape::ComputeDistance(const b2Transform& xf, const b2Vec2& p, float32* distance, b2Vec2* normal, int32 childIndex) const
{
B2_NOT_USED(childIndex);
b2Vec2 pLocal = b2MulT(xf.q, p - xf.p);
float32 maxDistance = -FLT_MAX;
b2Vec2 normalForMaxDistance = pLocal;
for (int32 i = 0; i < m_count; ++i)
{
float32 dot = b2Dot(m_normals[i], pLocal - m_vertices[i]);
if (dot > maxDistance)
{
maxDistance = dot;
normalForMaxDistance = m_normals[i];
}
}
if (maxDistance > 0)
{
b2Vec2 minDistance = normalForMaxDistance;
float32 minDistance2 = maxDistance * maxDistance;
for (int32 i = 0; i < m_count; ++i)
{
b2Vec2 distance = pLocal - m_vertices[i];
float32 distance2 = distance.LengthSquared();
if (minDistance2 > distance2)
{
minDistance = distance;
minDistance2 = distance2;
}
}
*distance = b2Sqrt(minDistance2);
*normal = b2Mul(xf.q, minDistance);
normal->Normalize();
}
else
{
*distance = maxDistance;
*normal = b2Mul(xf.q, normalForMaxDistance);
}
}
TEST_F(ConfinementTests, CircleShapes) {
b2CircleShape shape;
shape.m_radius = b2Sqrt(WIDTH * WIDTH + HEIGHT * HEIGHT);
shape.m_p = b2Vec2(2 * WIDTH, 0);
m_groundBody->CreateFixture(&shape, 0.0f);
shape.m_p = b2Vec2(-2 * WIDTH, 0);
m_groundBody->CreateFixture(&shape, 0.0f);
shape.m_p = b2Vec2(0, 2 * HEIGHT);
m_groundBody->CreateFixture(&shape, 0.0f);
shape.m_p = b2Vec2(0, -2 * HEIGHT);
m_groundBody->CreateFixture(&shape, 0.0f);
shape.m_p = b2Vec2(-WIDTH, -HEIGHT);
m_groundBody->CreateFixture(&shape, 0.0f);
shape.m_p = b2Vec2(WIDTH, -HEIGHT);
m_groundBody->CreateFixture(&shape, 0.0f);
shape.m_p = b2Vec2(-WIDTH, HEIGHT);
m_groundBody->CreateFixture(&shape, 0.0f);
shape.m_p = b2Vec2(WIDTH, HEIGHT);
m_groundBody->CreateFixture(&shape, 0.0f);
ASSERT_EQ(TestLeakCount(), 0);
}
float32 DistanceGeneric(b2Vec2* x1, b2Vec2* x2,
const T1* shape1, const b2XForm& xf1,
const T2* shape2, const b2XForm& xf2)
{
b2Vec2 p1s[3], p2s[3];
b2Vec2 points[3];
int32 pointCount = 0;
*x1 = shape1->GetFirstVertex(xf1);
*x2 = shape2->GetFirstVertex(xf2);
float32 vSqr = 0.0f;
const int32 maxIterations = 20;
for (int32 iter = 0; iter < maxIterations; ++iter)
{
b2Vec2 v = *x2 - *x1;
b2Vec2 w1 = shape1->Support(xf1, v);
b2Vec2 w2 = shape2->Support(xf2, -v);
vSqr = b2Dot(v, v);
b2Vec2 w = w2 - w1;
float32 vw = b2Dot(v, w);
if (vSqr - vw <= 0.01f * vSqr || InPoints(w, points, pointCount)) // or w in points
{
if (pointCount == 0)
{
*x1 = w1;
*x2 = w2;
}
g_GJK_Iterations = iter;
return b2Sqrt(vSqr);
}
switch (pointCount)
{
case 0:
p1s[0] = w1;
p2s[0] = w2;
points[0] = w;
*x1 = p1s[0];
*x2 = p2s[0];
++pointCount;
break;
case 1:
p1s[1] = w1;
p2s[1] = w2;
points[1] = w;
pointCount = ProcessTwo(x1, x2, p1s, p2s, points);
break;
case 2:
p1s[2] = w1;
p2s[2] = w2;
points[2] = w;
pointCount = ProcessThree(x1, x2, p1s, p2s, points);
break;
}
// If we have three points, then the origin is in the corresponding triangle.
if (pointCount == 3)
{
g_GJK_Iterations = iter;
return 0.0f;
}
float32 maxSqr = -B2_FLT_MAX;
for (int32 i = 0; i < pointCount; ++i)
{
maxSqr = b2Max(maxSqr, b2Dot(points[i], points[i]));
}
#ifdef TARGET_FLOAT32_IS_FIXED
if (pointCount == 3 || vSqr <= 5.0*B2_FLT_EPSILON * maxSqr)
#else
if (vSqr <= 100.0f * B2_FLT_EPSILON * maxSqr)
#endif
{
g_GJK_Iterations = iter;
v = *x2 - *x1;
vSqr = b2Dot(v, v);
return b2Sqrt(vSqr);
}
}
g_GJK_Iterations = maxIterations;
return b2Sqrt(vSqr);
}
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;
}
