void AdjustCloudNormal(pcl::PointCloud<PointT>::Ptr cloud, pcl::PointCloud<NormalT>::Ptr cloud_normals) { pcl::PointCloud<myPointXYZ>::Ptr center(new pcl::PointCloud<myPointXYZ>()); ComputeCentroid(cloud, center); myPointXYZ origin = center->at(0); std::cerr << origin.x << " " << origin.y << " " << origin.z << std::endl; pcl::transformPointCloud(*cloud, *cloud, Eigen::Vector3f(-origin.x, -origin.y, -origin.z), Eigen::Quaternion<float> (1,0,0,0)); int num = cloud->points.size(); float diffx, diffy, diffz, dist, theta; for( int j = 0; j < num ; j++ ) { PointT temp = cloud->at(j); NormalT temp_normals = cloud_normals->at(j); diffx = temp.x; diffy = temp.y; diffz = temp.z; dist = sqrt( diffx*diffx + diffy*diffy + diffz*diffz ); theta = acos( (diffx*temp_normals.normal_x + diffy*temp_normals.normal_y + diffz*temp_normals.normal_z)/dist ); if( theta > PI/2) { cloud_normals->at(j).normal_x = -cloud_normals->at(j).normal_x; cloud_normals->at(j).normal_y = -cloud_normals->at(j).normal_y; cloud_normals->at(j).normal_z = -cloud_normals->at(j).normal_z; } } }
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { b2Assert(3 <= count); m_vertexCount = count; delete m_vertices; m_vertices = new b2Vec2[m_vertexCount]; delete m_normals; m_normals = new b2Vec2[m_vertexCount]; // Copy vertices. for (int32 i = 0; i < m_vertexCount; ++i) { m_vertices[i] = vertices[i]; } // Compute normals. Ensure the edges have non-zero length. for (int32 i = 0; i < m_vertexCount; ++i) { int32 i1 = i; int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); m_normals[i] = b2Cross(edge, 1.0f); m_normals[i].Normalize(); } #ifdef _DEBUG // Ensure the polygon is convex and the interior // is to the left of each edge. for (int32 i = 0; i < m_vertexCount; ++i) { int32 i1 = i; int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; for (int32 j = 0; j < m_vertexCount; ++j) { // Don't check vertices on the current edge. if (j == i1 || j == i2) { continue; } b2Vec2 r = m_vertices[j] - m_vertices[i1]; // If this crashes, your polygon is non-convex, has colinear edges, // or the winding order is wrong. float32 s = b2Cross(edge, r); b2Assert(s > 0.0f && "ERROR: Please ensure your polygon is convex and has a CCW winding order"); } } #endif // Compute the polygon centroid. m_centroid = ComputeCentroid(m_vertices, m_vertexCount); }
void Mesh::ScaleZ(float s) { Vector c = ComputeCentroid(); Triangle *tri = tris; for(int i = 0; i < numTris; i++, tri++) { tri->ScaleZAboutPoint(s, c); } }
void Mesh::RotateAboutZ(float angle) { Vector c = ComputeCentroid(); Triangle *tri = tris; for(int i = 0; i < numTris; i++, tri++) { tri->RotateAboutZAboutPoint(angle, c); } rotation.z += angle; }
void CustomContactListener::applyBuoyancyEachStep() { if(!_isBuoyancyEnable) return; set<fixturePair>::iterator it = fixturePairs.begin(); set<fixturePair>::iterator end = fixturePairs.end(); while (it != end) { //fixtureA is the fluid b2Fixture* fixtureA = it->first; b2Fixture* fixtureB = it->second; float density = fixtureA->GetDensity(); vector<b2Vec2> intersectionPoints; if ( findIntersectionOfFixtures(fixtureA, fixtureB, intersectionPoints) ) { //find centroid float area = 0; b2Vec2 centroid = ComputeCentroid( intersectionPoints, area); //apply buoyancy stuff here... b2Vec2 gravity = physics->getWorld()->GetGravity(); //apply buoyancy force (fixtureA is the fluid) float displacedMass = fixtureA->GetDensity() * area; b2Vec2 buoyancyForce = displacedMass * - gravity; fixtureB->GetBody()->ApplyForce( buoyancyForce, centroid ); // //find relative velocity between object and fluid b2Vec2 velDir = fixtureB->GetBody()->GetLinearVelocityFromWorldPoint( centroid ) - fixtureA->GetBody()->GetLinearVelocityFromWorldPoint( centroid ); float vel = velDir.Normalize(); //apply simple linear drag float dragMag = fixtureA->GetDensity() * vel * vel; b2Vec2 dragForce = dragMag * -velDir; fixtureB->GetBody()->ApplyForce( dragForce, centroid ); //apply simple angular drag float angularDrag = area * -fixtureB->GetBody()->GetAngularVelocity(); fixtureB->GetBody()->ApplyTorque( angularDrag ); cout<<"apply buoyancy ..."<<endl; cout<<"density: "<<fixtureA->GetDensity()<<endl; cout<<"area: "<<area<<endl; cout<<"displaceMass: "<<displacedMass<<endl; cout<<"centroid: "<<centroid.x<<" "<<centroid.y<<endl; cout<<"buoyancyForce: "<<buoyancyForce.x<<" "<<buoyancyForce.y<<endl; } ++it; } }
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { b2Assert(2 <= count && count <= b2_maxPolygonVertices); m_vertexCount = count; // Copy vertices. for (int32 i = 0; i < m_vertexCount; ++i) { m_vertices[i] = vertices[i]; } // Compute normals. Ensure the edges have non-zero length. for (int32 i = 0; i < m_vertexCount; ++i) { int32 i1 = i; int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); m_normals[i] = b2Cross(edge, 1.0f); m_normals[i].Normalize(); } #ifdef _DEBUG // Ensure the polygon is convex and the interior // is to the left of each edge. for (int32 i = 0; i < m_vertexCount; ++i) { int32 i1 = i; int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; for (int32 j = 0; j < m_vertexCount; ++j) { // Don't check vertices on the current edge. if (j == i1 || j == i2) { continue; } b2Vec2 r = m_vertices[j] - m_vertices[i1]; // Your polygon is non-convex (it has an indentation) or // has colinear edges. float32 s = b2Cross(edge, r); b2Assert(s > 0.0f); } } #endif // Compute the polygon centroid. m_centroid = ComputeCentroid(m_vertices, m_vertexCount); }
void Triangle::RotateAboutX(float angle) { ComputeCentroid(); vertices[0].RotateAboutXAboutPoint(angle, centroid); vertices[1].RotateAboutXAboutPoint(angle, centroid); vertices[2].RotateAboutXAboutPoint(angle, centroid); float sa = sinf(angle); float ca = cosf(angle); float ny = normal.y; float nz = normal.z; normal.y = ca * ny - sa * nz; normal.z = sa * ny + ca * nz; }
void Mesh::SetRotation(Vector r) { Triangle *tri = tris, *ot = originalTris; Vector c = ComputeCentroid(); Vector o; for(int i = 0; i < numTris; i++, tri++, ot++) { *tri = *ot; tri->RotateAboutPoint(rotation + r, o); tri->Translate(c); tri->RecalcNormal(); } }
void Triangle::RotateAboutZ(float angle) { ComputeCentroid(); vertices[0].RotateAboutZAboutPoint(angle, centroid); vertices[1].RotateAboutZAboutPoint(angle, centroid); vertices[2].RotateAboutZAboutPoint(angle, centroid); float sa = sinf(angle); float ca = cosf(angle); float nx = normal.x; float ny = normal.y; normal.x = ca * nx - sa * ny; normal.y = sa * nx + ca * ny; }
void Mesh::Explode(float dist, float variance) { Vector c = ComputeCentroid(); Vector d; Triangle *tri = tris; float rand; variance *= 2; for(int i = 0; i < numTris; i++, tri++) { tri->ComputeCentroid(); d = tri->centroid - c; rand = (Random::randf() - 0.5f) * variance; d = d.Normalize() * (dist + rand); tri->Translate(d); } }
//---------------------------------------------------------------------------------- //! The process method is called by the parent class. //---------------------------------------------------------------------------------- void WEMCenterOfMass::_process() { ML_TRACE_IN("WEMCenterOfMass::process()") // for time measurement and mouse cursor setting. _startProcessing(); WEMInspector::_process(); // Compute centroid of WEM ComputeCentroid(); // stop time measurement and mouse cursor resetting. _finishProcessing(); // notify registered observer modules. //_notifyObservers(); }
void Triangle::Rotate(Vector rot) { ComputeCentroid(); vertices[0].RotateAboutPoint(rot, centroid); vertices[1].RotateAboutPoint(rot, centroid); vertices[2].RotateAboutPoint(rot, centroid); float cx = cosf(rot.x); float cy = cosf(rot.y); float cz = cosf(rot.z); float sx = sinf(rot.x); float sy = sinf(rot.y); float sz = sinf(rot.z); float nx = normal.x; float ny = normal.y; float nz = normal.z; normal.x = cy * cz * nx - sz * cy * ny - sy * nz; normal.y = (cx * sz - sx * sy * cz) * nx + (sx * sy * sz + cx * cz) * ny - sx * cy * nz; normal.z = (cx * sy * cz + sx * sz) * nx + (sx * cz - cx * sy * sz) * ny + cx * cy * nz; }
void Mesh::ReadFrom(FILE *fp) { //clear the current tris if(tris) { delete[] tris; tris = NULL; numTris = 0; } //get the number of triangles fread(&numTris, sizeof(int), 1, fp); tris = new Triangle[numTris]; originalTris = new Triangle[numTris]; //get all of the triangles Triangle *tri = tris, *ot = originalTris; for(int i = 0; i < numTris; i++, tri++, ot++) { tri->ReadFrom(fp); *ot = *tri; } centroid = ComputeCentroid(); }
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { b2Assert(3 <= count && count <= b2_maxPolygonVertices); if (count < 3) { SetAsBox(1.0f, 1.0f); return; } int32 n = b2Min(count, b2_maxPolygonVertices); // Perform welding and copy vertices into local buffer. b2Vec2 ps[b2_maxPolygonVertices]; int32 tempCount = 0; for (int32 i = 0; i < n; ++i) { b2Vec2 v = vertices[i]; bool unique = true; for (int32 j = 0; j < tempCount; ++j) { if (b2DistanceSquared(v, ps[j]) < 0.5f * b2_linearSlop) { unique = false; break; } } if (unique) { ps[tempCount++] = v; } } n = tempCount; if (n < 3) { // Polygon is degenerate. b2Assert(false); SetAsBox(1.0f, 1.0f); return; } // Create the convex hull using the Gift wrapping algorithm // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm // Find the right most point on the hull int32 i0 = 0; float32 x0 = ps[0].x; for (int32 i = 1; i < n; ++i) { float32 x = ps[i].x; if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) { i0 = i; x0 = x; } } int32 hull[b2_maxPolygonVertices]; int32 m = 0; int32 ih = i0; for (;;) { hull[m] = ih; int32 ie = 0; for (int32 j = 1; j < n; ++j) { if (ie == ih) { ie = j; continue; } b2Vec2 r = ps[ie] - ps[hull[m]]; b2Vec2 v = ps[j] - ps[hull[m]]; float32 c = b2Cross(r, v); if (c < 0.0f) { ie = j; } // Collinearity check if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) { ie = j; } } ++m; ih = ie; if (ie == i0) { break; } } if (m < 3) { // Polygon is degenerate. b2Assert(false); SetAsBox(1.0f, 1.0f); return; } m_count = m; // Copy vertices. for (int32 i = 0; i < m; ++i) { m_vertices[i] = ps[hull[i]]; } // Compute normals. Ensure the edges have non-zero length. for (int32 i = 0; i < m; ++i) { int32 i1 = i; int32 i2 = i + 1 < m ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); m_normals[i] = b2Cross(edge, 1.0f); m_normals[i].Normalize(); } // Compute the polygon centroid. m_centroid = ComputeCentroid(m_vertices, m); }
//----------------------------------------------------------------------------- //----------------------------------------------------------------------------- void CNPC_Blob::RunAI() { BaseClass::RunAI(); if( !m_bInitialized ) { // m_bInitialized is set to false in the constructor. So this bit of // code runs one time, the first time I think. Msg("I need to initialize\n"); InitializeElements(); m_bInitialized = true; return; } int iIdealNumElements = blob_numelements.GetInt(); if( iIdealNumElements != m_iNumElements ) { int delta = iIdealNumElements - m_iNumElements; if( delta < 0 ) { delta = -delta; delta = MIN(delta, 5 ); RemoveExcessElements( delta ); if( m_iReconfigureElement > m_iNumElements ) { // Start this index over at zero, if it is past the new end of the utlvector. m_iReconfigureElement = 0; } } else { delta = MIN(delta, 5 ); AddNewElements( delta ); } RecomputeIdealElementDist(); } ComputeCentroid(); if( npc_blob_show_centroid.GetBool() ) { NDebugOverlay::Cross3D( m_vecCentroid + Vector( 0, 0, 12 ), 32, 0, 255, 0, false, 0.025f ); } if( npc_blob_use_threading.GetBool() ) { IterRangeParallel( this, &CNPC_Blob::DoBlobBatchedAI, 0, m_Elements.Count() ); } else { DoBlobBatchedAI( 0, m_Elements.Count() ); } if( GetEnemy() != NULL ) { float flEnemyDistSqr = m_vecCentroid.DistToSqr( GetEnemy()->GetAbsOrigin() ); if( flEnemyDistSqr <= Square( 32.0f ) ) { if( GetEnemy()->Classify() == CLASS_COMBINE ) { if( !m_bEatCombineHack ) { variant_t var; var.SetFloat( 0 ); g_EventQueue.AddEvent( GetEnemy(), "HitByBugBait", 0.0f, this, this ); g_EventQueue.AddEvent( GetEnemy(), "SetHealth", var, 3.0f, this, this ); m_bEatCombineHack = true; blob_radius.SetValue( 48.0f ); RecomputeIdealElementDist(); } } else { CTakeDamageInfo info; info.SetAttacker( this ); info.SetInflictor( this ); info.SetDamage( 5 ); info.SetDamageType( DMG_SLASH ); info.SetDamageForce( Vector( 0, 0, 1 ) ); GetEnemy()->TakeDamage( info ); } } } SetNextThink( gpGlobals->curtime + npc_blob_think_interval.GetFloat() ); }
int main() { // expected (-5,2)/(5,-2)/(-5,2)/(5,2)/(5,2) (7.5,3) (2.5,1) cerr << RotateCCW90(PT(2,5)) << endl; cerr << RotateCW90(PT(2,5)) << endl; cerr << RotateCCW(PT(2,5),M_PI/2) << endl; cerr << ProjectPointLine(PT(-5,-2), PT(10,4), PT(3,7)) << endl; cerr << ProjectPointSegment(PT(-5,-2), PT(10,4), PT(3,7)) << " " << ProjectPointSegment(PT(7.5,3), PT(10,4), PT(3,7)) << " " << ProjectPointSegment(PT(-5,-2), PT(2.5,1), PT(3,7)) << endl; // expected 6.78903/1 0 1/0 0 1/1 1 1 0/(1,2)/(1,1) cerr << DistancePointPlane(4,-4,3,2,-2,5,-8) << endl; cerr << LinesParallel(PT(1,1), PT(3,5), PT(2,1), PT(4,5)) << " " << LinesParallel(PT(1,1), PT(3,5), PT(2,0), PT(4,5)) << " " << LinesParallel(PT(1,1), PT(3,5), PT(5,9), PT(7,13)) << endl; cerr << LinesCollinear(PT(1,1), PT(3,5), PT(2,1), PT(4,5)) << " " << LinesCollinear(PT(1,1), PT(3,5), PT(2,0), PT(4,5)) << " " << LinesCollinear(PT(1,1), PT(3,5), PT(5,9), PT(7,13)) << endl; cerr << SegmentsIntersect(PT(0,0), PT(2,4), PT(3,1), PT(-1,3)) << " " << SegmentsIntersect(PT(0,0), PT(2,4), PT(4,3), PT(0,5)) << " " << SegmentsIntersect(PT(0,0), PT(2,4), PT(2,-1), PT(-2,1)) << " " << SegmentsIntersect(PT(0,0), PT(2,4), PT(5,5), PT(1,7)) << endl; cerr << ComputeLineIntersection(PT(0,0),PT(2,4),PT(3,1),PT(-1,3)) << endl; cerr << ComputeCircleCenter(PT(-3,4), PT(6,1), PT(4,5)) << endl; vector<PT> v; v.push_back(PT(0,0)); v.push_back(PT(5,0)); v.push_back(PT(5,5)); v.push_back(PT(0,5)); // expected: 1 1 1 0 0 cerr << PointInPolygon(v, PT(2,2)) << " " << PointInPolygon(v, PT(2,0)) << " " << PointInPolygon(v, PT(0,2)) << " " << PointInPolygon(v, PT(5,2)) << " " << PointInPolygon(v, PT(2,5)) << endl; // expected: 0 1 1 1 1 cerr << PointOnPolygon(v, PT(2,2)) << " " << PointOnPolygon(v, PT(2,0)) << " " << PointOnPolygon(v, PT(0,2)) << " " << PointOnPolygon(v, PT(5,2)) << " " << PointOnPolygon(v, PT(2,5)) << endl; // expected: (1,6)/(5,4) (4,5)//(4,5) (5,4)//(4,5) (5,4) vector<PT> u = CircleLineIntersection(PT(0,6), PT(2,6), PT(1,1), 5); for (int i = 0; i < u.size(); i++) cerr << u[i] << " "; cerr << endl; u = CircleLineIntersection(PT(0,9), PT(9,0), PT(1,1), 5); for (int i = 0; i < u.size(); i++) cerr << u[i] << " "; cerr << endl; u = CircleCircleIntersection(PT(1,1), PT(10,10), 5, 5); for (int i = 0; i < u.size(); i++) cerr << u[i] << " "; cerr << endl; u = CircleCircleIntersection(PT(1,1), PT(8,8), 5, 5); for (int i = 0; i < u.size(); i++) cerr << u[i] << " "; cerr << endl; u = CircleCircleIntersection(PT(1,1), PT(4.5,4.5), 10, sqrt(2.0)/2.0); for (int i = 0; i < u.size(); i++) cerr << u[i] << " "; cerr << endl; u = CircleCircleIntersection(PT(1,1), PT(4.5,4.5), 5, sqrt(2.0)/2.0); for (int i = 0; i < u.size(); i++) cerr << u[i] << " "; cerr << endl; // area should be 5.0; centroid should be (1.1666666, 1.166666) PT pa[] = {PT(0,0), PT(5,0), PT(1,1), PT(0,5)}; vector<PT> p(pa, pa+4); PT c = ComputeCentroid(p); cerr << "Area: " << ComputeArea(p) << endl; cerr << "Centroid: " << c << endl; }
void b2PolygonShape::Set(const b2Vec2* vertices, int32 count) { if (count < 3) { SetAsBox(0.01f, 0.01f); return; } count = b2Min(count, b2_maxPolygonVertices); // Copy vertices into local buffer b2Vec2 ps[b2_maxPolygonVertices]; for (int32 i = 0; i < count; ++i) { ps[i] = vertices[i]; } // Create the convex hull using the Gift wrapping algorithm // http://en.wikipedia.org/wiki/Gift_wrapping_algorithm // Find the right most point on the hull int32 i0 = 0; float32 x0 = ps[0].x; for (int32 i = 1; i < count; ++i) { float32 x = ps[i].x; if (x > x0 || (x == x0 && ps[i].y < ps[i0].y)) { i0 = i; x0 = x; } } int32 hull[b2_maxPolygonVertices]; int32 m = 0; int32 ih = i0; for (;;) { hull[m] = ih; int32 ie = 0; for (int32 j = 1; j < count; ++j) { if (ie == ih) { ie = j; continue; } b2Vec2 r = ps[ie] - ps[hull[m]]; b2Vec2 v = ps[j] - ps[hull[m]]; float32 c = b2Cross(r, v); if (c < 0.0f) { ie = j; } // Collinearity check if (c == 0.0f && v.LengthSquared() > r.LengthSquared()) { ie = j; } } ++m; ih = ie; if (ie == i0) { break; } } if (m < 3) { SetAsBox(0.01f, 0.01f); return; } m_count = m; // Copy vertices. for (int32 i = 0; i < m; ++i) { m_vertices[i] = ps[hull[i]]; } // Compute normals. Ensure the edges have non-zero length. for (int32 i = 0; i < m; ++i) { int32 i1 = i; int32 i2 = i + 1 < m ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; b2Assert(edge.LengthSquared() > b2_epsilon * b2_epsilon); m_normals[i] = b2Cross(edge, 1.0f); m_normals[i].Normalize(); } // Compute the polygon centroid. m_centroid = ComputeCentroid(m_vertices, m); }
void PointCloud::ComputeCentroid() { centroid = ComputeCentroid(positions); }
b2PolygonShape::b2PolygonShape(const b2ShapeDef* def) : b2Shape(def) { b2Assert(def->type == e_polygonShape); m_type = e_polygonShape; const b2PolygonDef* poly = (const b2PolygonDef*)def; // Get the vertices transformed into the body frame. m_vertexCount = poly->vertexCount; b2Assert(3 <= m_vertexCount && m_vertexCount <= b2_maxPolygonVertices); // Copy vertices. for (int32 i = 0; i < m_vertexCount; ++i) { m_vertices[i] = poly->vertices[i]; } // Compute normals. Ensure the edges have non-zero length. for (int32 i = 0; i < m_vertexCount; ++i) { int32 i1 = i; int32 i2 = i + 1 < m_vertexCount ? i + 1 : 0; b2Vec2 edge = m_vertices[i2] - m_vertices[i1]; b2Assert(edge.LengthSquared() > B2_FLT_EPSILON * B2_FLT_EPSILON); m_normals[i] = b2Cross(edge, 1.0f); m_normals[i].Normalize(); } #ifdef _DEBUG // Ensure the polygon is convex. for (int32 i = 0; i < m_vertexCount; ++i) { for (int32 j = 0; j < m_vertexCount; ++j) { // Don't check vertices on the current edge. if (j == i || j == (i + 1) % m_vertexCount) { continue; } // Your polygon is non-convex (it has an indentation). // Or your polygon is too skinny. float32 s = b2Dot(m_normals[i], m_vertices[j] - m_vertices[i]); b2Assert(s < -b2_linearSlop); } } // Ensure the polygon is counter-clockwise. for (int32 i = 1; i < m_vertexCount; ++i) { float32 cross = b2Cross(m_normals[i-1], m_normals[i]); // Keep asinf happy. cross = b2Clamp(cross, -1.0f, 1.0f); // You have consecutive edges that are almost parallel on your polygon. float32 angle = asinf(cross); b2Assert(angle > b2_angularSlop); } #endif // Compute the polygon centroid. m_centroid = ComputeCentroid(poly->vertices, poly->vertexCount); // Compute the oriented bounding box. ComputeOBB(&m_obb, m_vertices, m_vertexCount); // Create core polygon shape by shifting edges inward. // Also compute the min/max radius for CCD. for (int32 i = 0; i < m_vertexCount; ++i) { int32 i1 = i - 1 >= 0 ? i - 1 : m_vertexCount - 1; int32 i2 = i; b2Vec2 n1 = m_normals[i1]; b2Vec2 n2 = m_normals[i2]; b2Vec2 v = m_vertices[i] - m_centroid;; b2Vec2 d; d.x = b2Dot(n1, v) - b2_toiSlop; d.y = b2Dot(n2, v) - b2_toiSlop; // Shifting the edge inward by b2_toiSlop should // not cause the plane to pass the centroid. // Your shape has a radius/extent less than b2_toiSlop. b2Assert(d.x >= 0.0f); b2Assert(d.y >= 0.0f); b2Mat22 A; A.col1.x = n1.x; A.col2.x = n1.y; A.col1.y = n2.x; A.col2.y = n2.y; m_coreVertices[i] = A.Solve(d) + m_centroid; } }