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; }