static void cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) { cpPolyShape *poly = (cpPolyShape *)shape; cpPolyShapeAxis *axes = poly->tAxes; cpVect *verts = poly->tVerts; int numVerts = poly->numVerts; for(int i=0; i<numVerts; i++){ cpVect n = axes[i].n; cpFloat an = cpvdot(a, n); if(axes[i].d > an) continue; cpFloat bn = cpvdot(b, n); cpFloat t = (axes[i].d - an)/(bn - an); if(t < 0.0f || 1.0f < t) continue; cpVect point = cpvlerp(a, b, t); cpFloat dt = -cpvcross(n, point); cpFloat dtMin = -cpvcross(n, verts[i]); cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]); if(dtMin <= dt && dt <= dtMax){ info->shape = shape; info->t = t; info->n = n; } } }
static cpFloat FindSteiner(int count, cpVect *verts, struct Notch notch) { cpFloat min = INFINITY; cpFloat feature = -1.0; for(int i=1; i<count-1; i++){ int index = (notch.i + i)%count; cpVect seg_a = verts[index]; cpVect seg_b = verts[Next(index, count)]; cpFloat thing_a = cpvcross(notch.n, cpvsub(seg_a, notch.v)); cpFloat thing_b = cpvcross(notch.n, cpvsub(seg_b, notch.v)); if(thing_a*thing_b <= 0.0){ cpFloat t = thing_a/(thing_a - thing_b); cpFloat dist = cpvdot(notch.n, cpvsub(cpvlerp(seg_a, seg_b, t), notch.v)); if(dist >= 0.0 && dist <= min){ min = dist; feature = index + t; } } } return feature; }
static void display(void) { demos[demoIndex].drawFunc(); if(!paused || step){ cpVect newPoint = cpvlerp(mouseBody->p, ChipmunkDemoMouse, 0.25f); mouseBody->v = cpvmult(cpvsub(newPoint, mouseBody->p), 60.0f); mouseBody->p = newPoint; demos[demoIndex].updateFunc(ticks); ticks++; step = cpFalse; } if(drawBBs) cpSpaceEachShape(space, drawShapeBB, NULL); drawInstructions(); drawInfo(); drawString(-300, -200, ChipmunkDemoMessageString); glutSwapBuffers(); glClear(GL_COLOR_BUFFER_BIT); }
cpFloat cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat r) { cpVect offset = cpvlerp(a, b, 0.5f); // This approximates the shape as a box for rounded segments, but it's quite close. cpFloat length = cpvdist(b, a) + 2.0f*r; return m*((length*length + 4.0f*r*r)/12.0f + cpvlengthsq(offset)); }
static struct cpShapeMassInfo cpSegmentShapeMassInfo(cpFloat mass, cpVect a, cpVect b, cpFloat r) { struct cpShapeMassInfo info = { mass, cpMomentForBox(1.0f, cpvdist(a, b) + 2.0f*r, 2.0f*r), // TODO is an approximation. cpvlerp(a, b, 0.5f), cpAreaForSegment(a, b, r), }; return info; }
static void cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) { struct cpSplittingPlane *planes = poly->planes; int count = poly->count; cpFloat r = poly->r; cpFloat rsum = r + r2; for(int i=0; i<count; i++){ cpVect n = planes[i].n; cpFloat an = cpvdot(a, n); cpFloat d = an - cpvdot(planes[i].v0, n) - rsum; if(d < 0.0f) continue; cpFloat bn = cpvdot(b, n); cpFloat t = d/(an - bn); if(t < 0.0f || 1.0f < t) continue; cpVect point = cpvlerp(a, b, t); cpFloat dt = cpvcross(n, point); cpFloat dtMin = cpvcross(n, planes[(i - 1 + count)%count].v0); cpFloat dtMax = cpvcross(n, planes[i].v0); if(dtMin <= dt && dt <= dtMax){ info->shape = (cpShape *)poly; info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2)); info->normal = n; info->alpha = t; } } // Also check against the beveled vertexes. if(rsum > 0.0f){ for(int i=0; i<count; i++){ cpSegmentQueryInfo circle_info = {NULL, b, cpvzero, 1.0f}; CircleSegmentQuery(&poly->shape, planes[i].v0, r, a, b, r2, &circle_info); if(circle_info.alpha < info->alpha) (*info) = circle_info; } } }
// Recursive implementatino of the GJK loop. static inline struct ClosestPoints GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration) { if(iteration > MAX_GJK_ITERATIONS) { cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); return ClosestPointsNew(v0, v1); } cpVect delta = cpvsub(v1.ab, v0.ab); // TODO: should this be an area2x check? if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) { // Origin is behind axis. Flip and try again. return GJKRecurse(ctx, v1, v0, iteration); } else { cpFloat t = ClosestT(v0.ab, v1.ab); cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t))); struct MinkowskiPoint p = Support(ctx, n); #if DRAW_GJK ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1)); cpVect c = cpvlerp(v0.ab, v1.ab, 0.5); ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1)); ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1)); #endif if( cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f && cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f ) { // The triangle v0, p, v1 contains the origin. Use EPA to find the MSA. cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration); return EPA(ctx, v0, p, v1); } else { if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) { // The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer. cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); return ClosestPointsNew(v0, v1); } else { // p was closer to the origin than our existing edge. // Need to figure out which existing point to drop. if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) { return GJKRecurse(ctx, v0, p, iteration + 1); } else { return GJKRecurse(ctx, p, v1, iteration + 1); } } } } }
static void cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info) { cpSegmentShape *seg = (cpSegmentShape *)shape; cpVect n = seg->tn; // flip n if a is behind the axis if(cpvdot(a, n) < cpvdot(seg->ta, n)) n = cpvneg(n); cpFloat an = cpvdot(a, n); cpFloat bn = cpvdot(b, n); cpFloat d = cpvdot(seg->ta, n) + seg->r; cpFloat t = (d - an)/(bn - an); if(0.0f < t && t < 1.0f) { cpVect point = cpvlerp(a, b, t); cpFloat dt = -cpvcross(seg->tn, point); cpFloat dtMin = -cpvcross(seg->tn, seg->ta); cpFloat dtMax = -cpvcross(seg->tn, seg->tb); if(dtMin < dt && dt < dtMax) { info->shape = shape; info->t = t; info->n = n; return; // don't continue on and check endcaps } } if(seg->r) { cpSegmentQueryInfo info1; info1.shape = NULL; cpSegmentQueryInfo info2; info2.shape = NULL; circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1); circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2); if(info1.shape && !info2.shape) { (*info) = info1; } else if(info2.shape && !info1.shape) { (*info) = info2; } else if(info1.shape && info2.shape) { if(info1.t < info2.t) { (*info) = info1; } else { (*info) = info2; } } } }
static inline struct ClosestPoints GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration) { if(iteration > MAX_GJK_ITERATIONS){ cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); return ClosestPointsNew(v0, v1); } cpVect delta = cpvsub(v1.ab, v0.ab); if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){ // Origin is behind axis. Flip and try again. return GJKRecurse(ctx, v1, v0, iteration + 1); } else { cpFloat t = ClosestT(v0.ab, v1.ab); cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t))); struct MinkowskiPoint p = Support(ctx, n); #if DRAW_GJK ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1)); cpVect c = cpvlerp(v0.ab, v1.ab, 0.5); ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1)); ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1)); #endif if( cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f && cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f ){ cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration); // The triangle v0, p, v1 contains the origin. Use EPA to find the MSA. return EPA(ctx, v0, p, v1); } else { // The new point must be farther along the normal than the existing points. if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){ cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration); return ClosestPointsNew(v0, v1); } else { if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){ return GJKRecurse(ctx, v0, p, iteration + 1); } else { return GJKRecurse(ctx, p, v1, iteration + 1); } } } } }
void Slice::ClipPoly(cpSpace *space, cpShape *shape, cpVect n, cpFloat dist) { cpBody *body = cpShapeGetBody(shape); int count = cpPolyShapeGetCount(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 = cpBodyLocalToWorld(body, cpPolyShapeGetVert(shape, j)); cpFloat a_dist = cpvdot(a, n) - dist; if(a_dist < 0.0){ clipped[clippedCount] = a; clippedCount++; } cpVect b = cpBodyLocalToWorld(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, 0.0f)*DENSITY; cpFloat moment = cpMomentForPoly(mass, clippedCount, clipped, cpvneg(centroid), 0.0f); cpBody *new_body = cpSpaceAddBody(space, cpBodyNew(mass, moment)); cpBodySetPosition(new_body, centroid); cpBodySetVelocity(new_body, cpBodyGetVelocityAtWorldPoint(body, centroid)); cpBodySetAngularVelocity(new_body, cpBodyGetAngularVelocity(body)); cpTransform transform = cpTransformTranslate(cpvneg(centroid)); cpShape *new_shape = cpSpaceAddShape(space, cpPolyShapeNew(new_body, clippedCount, clipped, transform, 0.0)); // Copy whatever properties you have set on the original shape that are important cpShapeSetFriction(new_shape, cpShapeGetFriction(shape)); }
static void display(void) { glClear(GL_COLOR_BUFFER_BIT); drawSpace(space, currDemo->drawOptions ? currDemo->drawOptions : &options); drawInstructions(); drawInfo(); drawString(-300, -210, messageString); glutSwapBuffers(); ticks++; cpVect newPoint = cpvlerp(mousePoint_last, mousePoint, 0.25f); mouseBody->p = newPoint; mouseBody->v = cpvmult(cpvsub(newPoint, mousePoint_last), 60.0f); mousePoint_last = newPoint; currDemo->updateFunc(ticks); }
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); }
static void display(void) { PrintStringBuffer[0] = 0; PrintStringCursor = PrintStringBuffer; glMatrixMode(GL_MODELVIEW); glLoadIdentity(); glScalef(scale, scale, 1.0); glTranslatef(translate_x, translate_y, 0.0); demos[demoIndex].drawFunc(); if(!paused || step){ cpVect newPoint = cpvlerp(mouseBody->p, ChipmunkDemoMouse, 0.25f); mouseBody->v = cpvmult(cpvsub(newPoint, mouseBody->p), 60.0f); mouseBody->p = newPoint; demos[demoIndex].updateFunc(ticks); ticks++; ChipmunkDemoTime = ticks/60.0; step = cpFalse; } if(drawBBs) cpSpaceEachShape(space, drawShapeBB, NULL); glMatrixMode(GL_MODELVIEW); glPushMatrix(); { // Draw the text at fixed positions, // but save the drawing matrix for the mouse picking glLoadIdentity(); drawInstructions(); drawInfo(); drawString(-300, -200, ChipmunkDemoMessageString); } glPopMatrix(); glutSwapBuffers(); glClear(GL_COLOR_BUFFER_BIT); }
static void cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info) { cpVect n = seg->tn; cpFloat d = cpvdot(cpvsub(seg->ta, a), n); cpFloat r = seg->r + r2; cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n); cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a); // Make the endpoints relative to 'a' and move them by the thickness of the segment. cpVect seg_a = cpvadd(seg->ta, seg_offset); cpVect seg_b = cpvadd(seg->tb, seg_offset); cpVect delta = cpvsub(b, a); if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){ cpFloat d_offset = d + (d > 0.0f ? -r : r); cpFloat ad = -d_offset; cpFloat bd = cpvdot(delta, n) - d_offset; if(ad*bd < 0.0f){ cpFloat t = ad/(ad - bd); info->shape = (cpShape *)seg; info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2)); info->normal = flipped_n; info->alpha = t; } } else if(r != 0.0f){ cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f}; cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f}; CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1); CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2); if(info1.alpha < info2.alpha){ (*info) = info1; } else { (*info) = info2; } } }
static void cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info) { cpSplittingPlane *axes = poly->tPlanes; cpVect *verts = poly->tVerts; int numVerts = poly->numVerts; cpFloat r = poly->r; for(int i=0; i<numVerts; i++){ cpVect n = axes[i].n; cpFloat an = cpvdot(a, n); cpFloat d = axes[i].d + r - an; if(d > 0.0f) continue; cpFloat bn = cpvdot(b, n); cpFloat t = d/(bn - an); if(t < 0.0f || 1.0f < t) continue; cpVect point = cpvlerp(a, b, t); cpFloat dt = -cpvcross(n, point); cpFloat dtMin = -cpvcross(n, verts[(i - 1 + numVerts)%numVerts]); cpFloat dtMax = -cpvcross(n, verts[i]); if(dtMin <= dt && dt <= dtMax){ info->shape = (cpShape *)poly; info->t = t; info->n = n; } } // Also check against the beveled vertexes. if(r > 0.0f){ for(int i=0; i<numVerts; i++){ cpSegmentQueryInfo circle_info = {NULL, 1.0f, cpvzero}; CircleSegmentQuery(&poly->shape, verts[i], r, a, b, &circle_info); if(circle_info.t < info->t) (*info) = circle_info; } } }
static void circleSegmentQuery(cpShape *shape, cpVect center, cpFloat r, cpVect a, cpVect b, cpSegmentQueryInfo *info) { // offset the line to be relative to the circle a = cpvsub(a, center); b = cpvsub(b, center); cpFloat qa = cpvdot(a, a) - 2.0f*cpvdot(a, b) + cpvdot(b, b); cpFloat qb = -2.0f*cpvdot(a, a) + 2.0f*cpvdot(a, b); cpFloat qc = cpvdot(a, a) - r*r; cpFloat det = qb*qb - 4.0f*qa*qc; if(det >= 0.0f){ cpFloat t = (-qb - cpfsqrt(det))/(2.0f*qa); if(0.0f<= t && t <= 1.0f){ info->shape = shape; info->t = t; info->n = cpvnormalize(cpvlerp(a, b, t)); } } }
/* Mouse handling is a bit tricky. We want the user to move * tiles using the mouse but because tiles are dynamic bodies * managed by Chipmunk2D, we cannot directly control them. * This is resolved by creating a pivot joint between an * invisible mouse body that we can control and the tile body * that we cannot directly control. */ static void apply_mouse_motion(struct state* state) { struct mouse m; update_mouse(&m); int w, h; get_screen_size(&w, &h); int x = m.x_position * w; int y = m.y_position * h; cpVect mouse_pos = cpv(x, y); cpVect new_point = cpvlerp(cpBodyGetPosition(state->mouse_body), mouse_pos, 0.25f); cpBodySetVelocity( state->mouse_body, cpvmult(cpvsub(new_point, cpBodyGetPosition(state->mouse_body)), 60.0f)); cpBodySetPosition(state->mouse_body, new_point); if (m.left_click && state->mouse_joint == NULL) { cpFloat radius = 5.0; cpPointQueryInfo info = { 0 }; cpShape* shape = cpSpacePointQueryNearest(state->space, mouse_pos, radius, GRAB_FILTER, &info); if (shape && cpBodyGetMass(cpShapeGetBody(shape)) < INFINITY) { cpVect nearest = (info.distance > 0.0f ? info.point : mouse_pos); cpBody* body = cpShapeGetBody(shape); state->mouse_joint = cpPivotJointNew2(state->mouse_body, body, cpvzero, cpBodyWorldToLocal(body, nearest)); cpConstraintSetMaxForce(state->mouse_joint, 5000000.0f); cpConstraintSetErrorBias(state->mouse_joint, cpfpow(1.0f - 0.15f, 60.0f)); cpSpaceAddConstraint(state->space, state->mouse_joint); } } if (m.left_click == false && state->mouse_joint != NULL) { cpSpaceRemoveConstraint(state->space, state->mouse_joint); cpConstraintFree(state->mouse_joint); state->mouse_joint = NULL; } }
cpBool Buoyancy::WaterPreSolve(cpArbiter *arb, cpSpace *space, void *ptr) { CP_ARBITER_GET_SHAPES(arb, water, poly); cpBody *body = cpShapeGetBody(poly); // Get the top of the water sensor bounding box to use as the water level. cpFloat level = cpShapeGetBB(water).t; // Clip the polygon against the water level int count = cpPolyShapeGetCount(poly); int clippedCount = 0; #ifdef _MSC_VER // MSVC is pretty much the only compiler in existence that doesn't support variable sized arrays. cpVect clipped[10]; #else cpVect clipped[count + 1]; #endif for(int i=0, j=count-1; i<count; j=i, i++){ cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, j)); cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, i)); if(a.y < level){ clipped[clippedCount] = a; clippedCount++; } cpFloat a_level = a.y - level; cpFloat b_level = b.y - level; if(a_level*b_level < 0.0f){ cpFloat t = cpfabs(a_level)/(cpfabs(a_level) + cpfabs(b_level)); clipped[clippedCount] = cpvlerp(a, b, t); clippedCount++; } } // Calculate buoyancy from the clipped polygon area cpFloat clippedArea = cpAreaForPoly(clippedCount, clipped, 0.0f); cpFloat displacedMass = clippedArea*FLUID_DENSITY; cpVect centroid = cpCentroidForPoly(clippedCount, clipped); cpDataPointer data = ptr; DrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f), data); DrawDot(5, centroid, RGBAColor(0, 0, 1, 1), data); cpFloat dt = cpSpaceGetCurrentTimeStep(space); cpVect g = cpSpaceGetGravity(space); // Apply the buoyancy force as an impulse. cpBodyApplyImpulseAtWorldPoint(body, cpvmult(g, -displacedMass*dt), centroid); // Apply linear damping for the fluid drag. cpVect v_centroid = cpBodyGetVelocityAtWorldPoint(body, centroid); cpFloat k = k_scalar_body(body, centroid, cpvnormalize(v_centroid)); cpFloat damping = clippedArea*FLUID_DRAG*FLUID_DENSITY; cpFloat v_coef = cpfexp(-damping*dt*k); // linear drag // cpFloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag cpBodyApplyImpulseAtWorldPoint(body, cpvmult(cpvsub(cpvmult(v_centroid, v_coef), v_centroid), 1.0/k), centroid); // Apply angular damping for the fluid drag. cpVect cog = cpBodyLocalToWorld(body, cpBodyGetCenterOfGravity(body)); cpFloat w_damping = cpMomentForPoly(FLUID_DRAG*FLUID_DENSITY*clippedArea, clippedCount, clipped, cpvneg(cog), 0.0f); cpBodySetAngularVelocity(body, cpBodyGetAngularVelocity(body)*cpfexp(-w_damping*dt/cpBodyGetMoment(body))); return cpTrue; }