void draw_shape(cpBody* body, cpShape* shape, void* data)
{
// get body info
cpVect v = cpBodyGetPos(body);
cpFloat angle = cpBodyGetAngle(body);
cpVect rot = cpvforangle(angle);
// get vectors
int n = cpPolyShapeGetNumVerts(shape);
SDL_Point* pts = calloc(sizeof(SDL_Point), n+1);
// rotate vectors
int i;
for(i=0; i<n; i++) {
cpVect p = cpPolyShapeGetVert(shape, i);
cpVect vr = cpvrotate(cpv(p.x,p.y), rot);
pts[i] = (SDL_Point) { (vr.x+v.x)*10+50, (vr.y+v.y)*10+50 };
if(i == 0)
pts[n] = pts[i];
}
// draw
SDL_RenderDrawLines(ren, pts, n+1);
free(pts);
}
int physics_poly_get_num_verts(Entity ent, unsigned int i)
{
PhysicsInfo *info;
info = entitypool_get(pool, ent);
error_assert(info);
return cpPolyShapeGetNumVerts(_get_shape(info, i)->shape);
}
cpVect
cpPolyShapeGetVert(cpShape *shape, int idx)
{
assert(shape->klass == &polyClass);
assert(idx < cpPolyShapeGetNumVerts(shape));
return ((cpPolyShape *)shape)->verts[idx];
}
cpVect
cpPolyShapeGetVert(cpShape *shape, int idx)
{
cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
cpAssert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");
return ((cpPolyShape *)shape)->verts[idx];
}
/* calculate moment for a single shape */
static Scalar _moment(cpBody *body, ShapeInfo *shapeInfo)
{
Scalar mass = cpBodyGetMass(body);
switch (shapeInfo->type)
{
case PS_CIRCLE:
return cpMomentForCircle(mass, 0,
cpCircleShapeGetRadius(shapeInfo->shape),
cpCircleShapeGetOffset(shapeInfo->shape));
case PS_POLYGON:
return cpMomentForPoly(mass,
cpPolyShapeGetNumVerts(shapeInfo->shape),
((cpPolyShape *) shapeInfo->shape)->verts,
cpvzero);
}
}
static void
draw_poly_shape(struct draw_options opts, cpShape *shape, cpBody *body)
{
int num_verts = cpPolyShapeGetNumVerts(shape);
cpVect first_vert = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, 0));
cpVect prev_vert = first_vert;
cpVect pos = cpBodyGetPos(body);
for (int i = 1; i < num_verts; i++)
{
cpVect vert = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, i));
draw_line(opts, prev_vert, vert);
prev_vert = vert;
}
/* close up the polygon */
draw_line(opts, prev_vert, first_vert);
}
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
ClipPoly(cpSpace *space, cpShape *shape, cpVect n, cpFloat dist)
{
cpBody *body = cpShapeGetBody(shape);
int count = cpPolyShapeGetNumVerts(shape);
int clippedCount = 0;
cpVect *clipped = (cpVect *)alloca((count + 1)*sizeof(cpVect));
for(int i=0, j=count-1; i<count; j=i, i++){
cpVect a = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, j));
cpFloat a_dist = cpvdot(a, n) - dist;
if(a_dist < 0.0){
clipped[clippedCount] = a;
clippedCount++;
}
cpVect b = cpBodyLocal2World(body, cpPolyShapeGetVert(shape, i));
cpFloat b_dist = cpvdot(b, n) - dist;
if(a_dist*b_dist < 0.0f){
cpFloat t = cpfabs(a_dist)/(cpfabs(a_dist) + cpfabs(b_dist));
clipped[clippedCount] = cpvlerp(a, b, t);
clippedCount++;
}
}
cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
cpFloat mass = cpAreaForPoly(clippedCount, clipped)*DENSITY;
cpFloat moment = cpMomentForPoly(mass, clippedCount, clipped, cpvneg(centroid));
cpBody *new_body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpBodySetPos(new_body, centroid);
cpBodySetVel(new_body, cpBodyGetVelAtWorldPoint(body, centroid));
cpBodySetAngVel(new_body, cpBodyGetAngVel(body));
cpShape *new_shape = cpSpaceAddShape(space, cpPolyShapeNew(new_body, clippedCount, clipped, cpvneg(centroid)));
// Copy whatever properties you have set on the original shape that are important
cpShapeSetFriction(new_shape, cpShapeGetFriction(shape));
}
void PhysicsShapePolygon::updateScale()
{
cpFloat factorX = PhysicsHelper::float2cpfloat(_newScaleX / _scaleX);
cpFloat factorY = PhysicsHelper::float2cpfloat(_newScaleY / _scaleY);
auto shape = _cpShapes.front();
int count = cpPolyShapeGetNumVerts(shape);
cpVect* vects = ((cpPolyShape*)shape)->verts;
cpSplittingPlane* planes = ((cpPolyShape*)shape)->planes;
for (int i = 0; i < count; ++i)
{
vects[i].x *= factorX;
vects[i].y *= factorY;
}
// convert hole to clockwise
if (factorX * factorY < 0)
{
for (int i = 0; i < count / 2; ++i)
{
cpVect v = vects[i];
vects[i] = vects[count - i - 1];
vects[count - i - 1] = v;
}
}
for (int i = 0; i < count; ++i)
{
// cpVect n = cpvnormalize(cpvperp(cpvsub(vects[i], vects[(i + 1) % count])));
//
// planes[i].n = n;
// planes[i].d = cpvdot(n, vects[i]);
// FIXED ME: if update 'planes[i]' as the above codes, then can not query polygon shape by PhysicsWorld::getShapes().
// But modified like this, then ray test can not work correctly on some cases.
planes[i].d = cpvdot(planes[i].n, vects[i]);
}
PhysicsShape::updateScale();
}
static int cpPolyShape_getNumVerts (lua_State *L) {
cpPolyShape* ps = check_cpPolyShape(L, 1);
lua_pushinteger(L, cpPolyShapeGetNumVerts((cpShape *)ps));
return 1;
}
