cpPolyline *
cpPolylineToConvexHull(cpPolyline *line, cpFloat tol)
{
cpPolyline *hull = cpPolylineMake(line->count + 1);
hull->count = cpConvexHull(line->count, line->verts, hull->verts, NULL, tol);
hull = cpPolylinePush(hull, hull->verts[0]);
return cpPolylineShrink(hull);
}
// Find the closest points between two shapes using the GJK algorithm.
static struct ClosestPoints
GJK(const struct SupportContext *ctx, cpCollisionID *id)
{
#if DRAW_GJK || DRAW_EPA
int count1 = 1;
int count2 = 1;
switch(ctx->shape1->klass->type) {
case CP_SEGMENT_SHAPE:
count1 = 2;
break;
case CP_POLY_SHAPE:
count1 = ((cpPolyShape *)ctx->shape1)->count;
break;
default:
break;
}
switch(ctx->shape2->klass->type) {
case CP_SEGMENT_SHAPE:
count1 = 2;
break;
case CP_POLY_SHAPE:
count2 = ((cpPolyShape *)ctx->shape2)->count;
break;
default:
break;
}
// draw the minkowski difference origin
cpVect origin = cpvzero;
ChipmunkDebugDrawDot(5.0, origin, RGBAColor(1,0,0,1));
int mdiffCount = count1*count2;
cpVect *mdiffVerts = alloca(mdiffCount*sizeof(cpVect));
for(int i=0; i<count1; i++) {
for(int j=0; j<count2; j++) {
cpVect v = cpvsub(ShapePoint(ctx->shape2, j).p, ShapePoint(ctx->shape1, i).p);
mdiffVerts[i*count2 + j] = v;
ChipmunkDebugDrawDot(2.0, v, RGBAColor(1, 0, 0, 1));
}
}
cpVect *hullVerts = alloca(mdiffCount*sizeof(cpVect));
int hullCount = cpConvexHull(mdiffCount, mdiffVerts, hullVerts, NULL, 0.0);
ChipmunkDebugDrawPolygon(hullCount, hullVerts, 0.0, RGBAColor(1, 0, 0, 1), RGBAColor(1, 0, 0, 0.25));
#endif
struct MinkowskiPoint v0, v1;
if(*id) {
// Use the minkowski points from the last frame as a starting point using the cached indexes.
v0 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>>24)&0xFF), ShapePoint(ctx->shape2, (*id>>16)&0xFF));
v1 = MinkowskiPointNew(ShapePoint(ctx->shape1, (*id>> 8)&0xFF), ShapePoint(ctx->shape2, (*id )&0xFF));
} else {
void
cpPolyShapeSetVerts(cpShape *shape, int count, cpVect *verts, cpTransform transform)
{
cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect));
// Transform the verts before building the hull in case of a negative scale.
for(int i=0; i<count; i++) hullVerts[i] = cpTransformPoint(transform, verts[i]);
unsigned int hullCount = cpConvexHull(count, hullVerts, hullVerts, NULL, 0.0);
cpPolyShapeSetVertsRaw(shape, hullCount, hullVerts);
}
cpPolyShape *
cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int count, const cpVect *verts, cpTransform transform, cpFloat radius)
{
cpVect *hullVerts = (cpVect *)alloca(count*sizeof(cpVect));
// Transform the verts before building the hull in case of a negative scale.
for(int i=0; i<count; i++) hullVerts[i] = cpTransformPoint(transform, verts[i]);
unsigned int hullCount = cpConvexHull(count, hullVerts, hullVerts, NULL, 0.0);
return cpPolyShapeInitRaw(poly, body, hullCount, hullVerts, radius);
}
unsigned int physics_convex_hull(unsigned int nverts, Vec2 *verts)
{
cpVect *cpverts;
unsigned int i;
cpverts = malloc(nverts * sizeof(cpVect));
for (i = 0; i < nverts; ++i)
cpverts[i] = cpv_of_vec2(verts[i]);
nverts = cpConvexHull(nverts, cpverts, NULL, NULL, 0);
for (i = 0; i < nverts; ++i)
verts[i] = vec2_of_cpv(cpverts[i]);
free(cpverts);
return nverts;
}
unsigned int physics_shape_add_poly(Entity ent,
unsigned int nverts,
const Vec2 *verts,
Scalar r)
{
unsigned int i;
cpVect *cpverts;
cpShape *shape;
cpverts = malloc(nverts * sizeof(cpVect));
for (i = 0; i < nverts; ++i)
cpverts[i] = cpv_of_vec2(verts[i]);
nverts = cpConvexHull(nverts, cpverts, NULL, NULL, 0);
shape = cpPolyShapeNew2(NULL, nverts, cpverts, cpvzero, r);
free(cpverts);
return _shape_add(ent, PS_POLYGON, shape);
}
static void
update(cpSpace *space)
{
cpFloat tolerance = 2.0;
if(ChipmunkDemoRightClick && cpShapeNearestPointQuery(shape, ChipmunkDemoMouse, NULL) > tolerance){
cpBody *body = cpShapeGetBody(shape);
int count = cpPolyShapeGetNumVerts(shape);
// Allocate the space for the new vertexes on the stack.
cpVect *verts = (cpVect *)alloca((count + 1)*sizeof(cpVect));
for(int i=0; i<count; i++){
verts[i] = cpPolyShapeGetVert(shape, i);
}
verts[count] = cpBodyWorld2Local(body, ChipmunkDemoMouse);
// This function builds a convex hull for the vertexes.
// Because the result array is NULL, it will reduce the input array instead.
int hullCount = cpConvexHull(count + 1, verts, NULL, NULL, tolerance);
// Figure out how much to shift the body by.
cpVect centroid = cpCentroidForPoly(hullCount, verts);
// Recalculate the body properties to match the updated shape.
cpFloat mass = cpAreaForPoly(hullCount, verts)*DENSITY;
cpBodySetMass(body, mass);
cpBodySetMoment(body, cpMomentForPoly(mass, hullCount, verts, cpvneg(centroid)));
cpBodySetPos(body, cpBodyLocal2World(body, centroid));
// Use the setter function from chipmunk_unsafe.h.
// You could also remove and recreate the shape if you wanted.
cpPolyShapeSetVerts(shape, hullCount, verts, cpvneg(centroid));
}
int steps = 1;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
}
}
static void
ApproximateConcaveDecomposition(cpVect *verts, int count, cpFloat tol, cpPolylineSet *set)
{
int first;
cpVect *hullVerts = alloca(count*sizeof(cpVect));
int hullCount = cpConvexHull(count, verts, hullVerts, &first, 0.0);
if(hullCount != count){
struct Notch notch = DeepestNotch(count, verts, hullCount, hullVerts, first, tol);
if(notch.d > tol){
cpFloat steiner_it = FindSteiner(count, verts, notch);
if(steiner_it >= 0.0){
int steiner_i = (int)steiner_it;
cpVect steiner = cpvlerp(verts[steiner_i], verts[Next(steiner_i, count)], steiner_it - steiner_i);
// Vertex counts NOT including the steiner point.
int sub1_count = (steiner_i - notch.i + count)%count + 1;
int sub2_count = count - (steiner_i - notch.i + count)%count;
cpVect *scratch = alloca((IMAX(sub1_count, sub2_count) + 1)*sizeof(cpVect));
for(int i=0; i<sub1_count; i++) scratch[i] = verts[(notch.i + i)%count];
scratch[sub1_count] = steiner;
ApproximateConcaveDecomposition(scratch, sub1_count + 1, tol, set);
for(int i=0; i<sub2_count; i++) scratch[i] = verts[(steiner_i + 1 + i)%count];
scratch[sub2_count] = steiner;
ApproximateConcaveDecomposition(scratch, sub2_count + 1, tol, set);
return;
}
}
}
cpPolyline *hull = cpPolylineMake(hullCount + 1);
memcpy(hull->verts, hullVerts, hullCount*sizeof(cpVect));
hull->verts[hullCount] = hullVerts[0];
hull->count = hullCount + 1;
cpPolylineSetPush(set, hull);
}
void ColliderDetector::updateTransform(AffineTransform &t)
{
if (!m_bActive)
{
return;
}
for(auto object : *m_pColliderBodyList)
{
ColliderBody *colliderBody = (ColliderBody *)object;
ContourData *contourData = colliderBody->getContourData();
b2PolygonShape* b2shape = nullptr;
if (m_pB2Body != nullptr)
{
b2shape = (b2PolygonShape *)colliderBody->getB2Fixture()->GetShape();
}
cpPolyShape* cpshape = nullptr;
if (m_pCPBody != nullptr)
{
cpshape = (cpPolyShape *)colliderBody->getShape();
}
int num = contourData->m_tVertexList.count();
ContourVertex2 **vs = (ContourVertex2 **)contourData->m_tVertexList.data->arr;
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
ContourVertex2 **cvs = (ContourVertex2 **)colliderBody->getCalculatedVertexList()->data->arr;
#endif
for (int i = 0; i < num; i++)
{
helpPoint.setPoint( vs[i]->x, vs[i]->y);
helpPoint = PointApplyAffineTransform(helpPoint, t);
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
cvs[i]->x = helpPoint.x;
cvs[i]->y = helpPoint.y;
#endif
if ( b2shape != nullptr )
{
b2Vec2 &bv = b2shape->m_vertices[i];
bv.Set(helpPoint.x / PT_RATIO, helpPoint.y / PT_RATIO);
}
if ( cpshape != nullptr )
{
cpVect v ;
v.x = helpPoint.x;
v.y = helpPoint.y;
cpshape->verts[i] = v;
}
}
if ( cpshape != nullptr )
{
cpConvexHull(num, cpshape->verts, nullptr, nullptr, 0);
for (int i = 0; i < num; i++)
{
cpVect b = cpshape->verts[(i + 1) % cpshape->numVerts];
cpVect n = cpvnormalize(cpvperp(cpvsub(b, cpshape->verts[i])));
cpshape->planes[i].n = n;
cpshape->planes[i].d = cpvdot(n, cpshape->verts[i]);
}
}
}
}
void ColliderDetector::updateTransform(Mat4 &t)
{
if (!_active)
{
return;
}
for(auto& object : _colliderBodyList)
{
ColliderBody *colliderBody = (ColliderBody *)object;
ContourData *contourData = colliderBody->getContourData();
#if ENABLE_PHYSICS_BOX2D_DETECT
b2PolygonShape *shape = nullptr;
if (_body != nullptr)
{
shape = (b2PolygonShape *)colliderBody->getB2Fixture()->GetShape();
}
#elif ENABLE_PHYSICS_CHIPMUNK_DETECT
cpPolyShape *shape = nullptr;
if (_body != nullptr)
{
shape = (cpPolyShape *)colliderBody->getShape();
}
#endif
unsigned long num = contourData->vertexList.size();
std::vector<cocos2d::Vec2> &vs = contourData->vertexList;
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
std::vector<cocos2d::Vec2> &cvs = colliderBody->_calculatedVertexList;
#endif
for (unsigned long i = 0; i < num; i++)
{
helpPoint.setPoint( vs.at(i).x, vs.at(i).y);
helpPoint = PointApplyTransform(helpPoint, t);
#if ENABLE_PHYSICS_SAVE_CALCULATED_VERTEX
cvs.at(i).x = helpPoint.x;
cvs.at(i).y = helpPoint.y;
#endif
#if ENABLE_PHYSICS_BOX2D_DETECT
if (shape != nullptr)
{
b2Vec2 &bv = shape->m_vertices[i];
bv.Set(helpPoint.x / PT_RATIO, helpPoint.y / PT_RATIO);
}
#elif ENABLE_PHYSICS_CHIPMUNK_DETECT
if (shape != nullptr)
{
cpVect v ;
v.x = helpPoint.x;
v.y = helpPoint.y;
shape->verts[i] = v;
}
#endif
}
#if ENABLE_PHYSICS_CHIPMUNK_DETECT
cpConvexHull((int)num, shape->verts, nullptr, nullptr, 0);
for (unsigned long i = 0; i < num; i++)
{
cpVect b = shape->verts[(i + 1) % shape->numVerts];
cpVect n = cpvnormalize(cpvperp(cpvsub(b, shape->verts[i])));
shape->planes[i].n = n;
shape->planes[i].d = cpvdot(n, shape->verts[i]);
}
#endif
}
}
