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