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