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