void Map::createMap()
{
//TODO : Add a quadrant system
const int w = m_pSurface->w / World::ratio;
const int h = m_pSurface->h / World::ratio;
b2Vec2 oldPos(0.0f,0.f);
const float minDis = 3;
for(unsigned int i=0; i<16*5; i++)
{
bool foundPos = false;
while(foundPos == false)
{
b2Vec2 pos(rand()%w, rand()%h);
if(b2DistanceSquared(pos, oldPos) >= minDis)
{
foundPos = true;
oldPos = pos;
}
}
Obstacle * obs = new Obstacle(m_pWorld, oldPos);
obs->init();
m_Obstacles.push_back(obs);
}
}
void b2ChainShape::CreateLoop(const b2Vec2* vertices, int32 count)
{
b2Assert(m_vertices == NULL && m_count == 0);
b2Assert(count >= 3);
if (count < 3)
{
return;
}
for (int32 i = 1; i < count; ++i)
{
b2Vec2 v1 = vertices[i-1];
b2Vec2 v2 = vertices[i];
// If the code crashes here, it means your vertices are too close together.
b2Assert(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop);
}
m_count = count + 1;
m_vertices = (b2Vec2*)b2Alloc(m_count * sizeof(b2Vec2));
memcpy(m_vertices, vertices, count * sizeof(b2Vec2));
m_vertices[count] = m_vertices[0];
m_prevVertex = m_vertices[m_count - 2];
m_nextVertex = m_vertices[1];
m_hasPrevVertex = true;
m_hasNextVertex = true;
}
void Initialize(b2ContactConstraint* cc)
{
b2Assert(cc->pointCount > 0);
switch (cc->type)
{
case b2Manifold::e_circles:
{
b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->points[0].localPoint);
if (b2DistanceSquared(pointA, pointB) > B2_FLT_EPSILON * B2_FLT_EPSILON)
{
m_normal = pointB - pointA;
m_normal.Normalize();
}
else
{
m_normal.Set(1.0f, 0.0f);
}
m_points[0] = 0.5f * (pointA + pointB);
m_separations[0] = b2Dot(pointB - pointA, m_normal) - cc->radius;
}
break;
case b2Manifold::e_faceA:
{
m_normal = cc->bodyA->GetWorldVector(cc->localPlaneNormal);
b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
for (int32 i = 0; i < cc->pointCount; ++i)
{
b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->points[i].localPoint);
m_separations[i] = b2Dot(clipPoint - planePoint, m_normal) - cc->radius;
m_points[i] = clipPoint;
}
}
break;
case b2Manifold::e_faceB:
{
m_normal = cc->bodyB->GetWorldVector(cc->localPlaneNormal);
b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
for (int32 i = 0; i < cc->pointCount; ++i)
{
b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->points[i].localPoint);
m_separations[i] = b2Dot(clipPoint - planePoint, m_normal) - cc->radius;
m_points[i] = clipPoint;
}
// Ensure normal points from A to B
m_normal = -m_normal;
}
break;
}
}
void Initialize(b2TOIConstraint* cc, int32 index)
{
b2Assert(cc->pointCount > 0);
switch (cc->type)
{
case b2Manifold::e_circles:
{
b2Vec2 pointA = cc->bodyA->GetWorldPoint(cc->localPoint);
b2Vec2 pointB = cc->bodyB->GetWorldPoint(cc->localPoints[0]);
if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
else
{
normal.Set(1.0f, 0.0f);
}
point = 0.5f * (pointA + pointB);
separation = b2Dot(pointB - pointA, normal) - cc->radius;
}
break;
case b2Manifold::e_faceA:
{
normal = cc->bodyA->GetWorldVector(cc->localNormal);
b2Vec2 planePoint = cc->bodyA->GetWorldPoint(cc->localPoint);
b2Vec2 clipPoint = cc->bodyB->GetWorldPoint(cc->localPoints[index]);
separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
point = clipPoint;
}
break;
case b2Manifold::e_faceB:
{
normal = cc->bodyB->GetWorldVector(cc->localNormal);
b2Vec2 planePoint = cc->bodyB->GetWorldPoint(cc->localPoint);
b2Vec2 clipPoint = cc->bodyA->GetWorldPoint(cc->localPoints[index]);
separation = b2Dot(clipPoint - planePoint, normal) - cc->radius;
point = clipPoint;
// Ensure normal points from A to B
normal = -normal;
}
break;
}
}
void b2ChainShape::CreateChain(const b2Vec2* vertices, int32 count)
{
b2Assert(m_vertices == NULL && m_count == 0);
b2Assert(count >= 2);
for (int32 i = 1; i < count; ++i)
{
// If the code crashes here, it means your vertices are too close together.
b2Assert(b2DistanceSquared(vertices[i-1], vertices[i]) > b2_linearSlop * b2_linearSlop);
}
m_count = count;
m_vertices = (b2Vec2*)b2Alloc(count * sizeof(b2Vec2));
memcpy(m_vertices, vertices, m_count * sizeof(b2Vec2));
m_hasPrevVertex = false;
m_hasNextVertex = false;
m_prevVertex.SetZero();
m_nextVertex.SetZero();
}
bool LHBodyShape::initChainWithDictionaryAndPoints(ValueMap& dict, const std::vector<Point>& points, bool close, b2Body* body, Node* node, LHScene* scene, float scaleX, float scaleY)
{
{
shapeName = dict["name"].asString();
shapeID = dict["shapeID"].asInt();
std::vector< b2Vec2 > verts;
LHValue* firstPt = nullptr;
LHValue* lastPt = nullptr;
for(int i = 0; i < points.size(); ++i)
{
Point pt = points[i];
pt.x *= scaleX;
pt.y *= scaleY;
b2Vec2 v2 = scene->metersFromPoint(pt);
if(lastPt != nullptr)
{
Point oldPt = lastPt->getPoint();
b2Vec2 v1 = b2Vec2(oldPt.x, oldPt.y);
if(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop)
{
verts.push_back(v2);
}
}
else{
verts.push_back(v2);
}
lastPt = LHValue::create(Point(v2.x, v2.y));
if(firstPt == nullptr)
{
firstPt = LHValue::create(Point(v2.x, v2.y));
}
}
if(firstPt && lastPt && close)
{
Point lastPoint = lastPt->getPoint();
b2Vec2 v1 = b2Vec2(lastPoint.x, lastPoint.y);
Point firstPoint = firstPt->getPoint();
b2Vec2 v2 = b2Vec2(firstPoint.x, firstPoint.y);
if(b2DistanceSquared(v1, v2) > b2_linearSlop * b2_linearSlop)
{
verts.push_back(v2);
}
}
b2Shape* shape = new b2ChainShape();
((b2ChainShape*)shape)->CreateChain (&(verts.front()), (int)verts.size());
b2FixtureDef fixture;
LHSetupb2FixtureWithInfo(&fixture, dict);
fixture.userData = this;
fixture.shape = shape;
body->CreateFixture(&fixture);
delete shape;
shape = NULL;
return true;
}
return false;
}
bool LHBodyShape::LHValidateCentroid(b2Vec2* vs, int count)
{
if(count > b2_maxPolygonVertices)
return false;
if(count < 3)
return false;
b2Vec2 c; c.Set(0.0f, 0.0f);
float32 area = 0.0f;
// pRef is the reference point for forming triangles.
// It's location doesn't change the result (except for rounding error).
b2Vec2 pRef(0.0f, 0.0f);
#if 0
// This code would put the reference point inside the polygon.
for (int32 i = 0; i < count; ++i)
{
pRef += vs[i];
}
pRef *= 1.0f / count;
#endif
const float32 inv3 = 1.0f / 3.0f;
for (int32 i = 0; i < count; ++i)
{
// Triangle vertices.
b2Vec2 p1 = pRef;
b2Vec2 p2 = vs[i];
b2Vec2 p3 = i + 1 < count ? vs[i+1] : vs[0];
b2Vec2 e1 = p2 - p1;
b2Vec2 e2 = p3 - p1;
float32 D = b2Cross(e1, e2);
float32 triangleArea = 0.5f * D;
area += triangleArea;
// Area weighted centroid
c += triangleArea * inv3 * (p1 + p2 + p3);
}
// Centroid
if(area < b2_epsilon)
{
return false;
}
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 = vs[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)
{
return false;
}
return true;
}
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 b2WorldManifold::Initialize(const b2Manifold* manifold,
const b2Transform& xfA, float32 radiusA,
const b2Transform& xfB, float32 radiusB)
{
if (manifold->pointCount == 0)
{
return;
}
switch (manifold->type)
{
case b2Manifold::e_circles:
{
normal.Set(1.0f, 0.0f);
b2Vec2 pointA = b2Mul(xfA, manifold->localPoint);
b2Vec2 pointB = b2Mul(xfB, manifold->points[0].localPoint);
if (b2DistanceSquared(pointA, pointB) > b2_epsilon * b2_epsilon)
{
normal = pointB - pointA;
normal.Normalize();
}
b2Vec2 cA = pointA + radiusA * normal;
b2Vec2 cB = pointB - radiusB * normal;
points[0] = 0.5f * (cA + cB);
separations[0] = b2Dot(cB - cA, normal);
}
break;
case b2Manifold::e_faceA:
{
normal = b2Mul(xfA.q, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfA, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
{
b2Vec2 clipPoint = b2Mul(xfB, manifold->points[i].localPoint);
b2Vec2 cA = clipPoint + (radiusA - b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cB = clipPoint - radiusB * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = b2Dot(cB - cA, normal);
}
}
break;
case b2Manifold::e_faceB:
{
normal = b2Mul(xfB.q, manifold->localNormal);
b2Vec2 planePoint = b2Mul(xfB, manifold->localPoint);
for (int32 i = 0; i < manifold->pointCount; ++i)
{
b2Vec2 clipPoint = b2Mul(xfA, manifold->points[i].localPoint);
b2Vec2 cB = clipPoint + (radiusB - b2Dot(clipPoint - planePoint, normal)) * normal;
b2Vec2 cA = clipPoint - radiusA * normal;
points[i] = 0.5f * (cA + cB);
separations[i] = b2Dot(cA - cB, normal);
}
// Ensure normal points from A to B.
normal = -normal;
}
break;
}
}
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;
}
}
// 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;
}
}
