cpPolyShapeCacheData(cpPolyShape poly, cpVect p, cpVect rot)
{
cpPolyShapeTransformAxes(poly, p, rot);
cpBB bb = poly.shape.bb = cpPolyShapeTransformVerts(poly, p, rot);
return bb;
}
protected override float CalculateArea()
{
cpPolyShape shape = (cpPolyShape)_info.GetShapes().FirstOrDefault(); //.front();
shape.CacheBB();
return(cp.AreaForPolyOld(shape.Count, shape.GetVertices()));
}
public void ShatterCell(cpPolyShape shape, cpVect cell, int cell_i, int cell_j, ref WorleyContex context)
{
// printf("cell %dx%d: (% 5.2f, % 5.2f)\n", cell_i, cell_j, cell.x, cell.y);
cpBody body = shape.body; // cpShapeGetBody(shape);
cpVect[] ping = new cpVect[MAX_VERTEXES_PER_VORONOI]; // cpVect[ (cpVect*)alloca( * sizeof(cpVect));
cpVect[] pong = new cpVect[MAX_VERTEXES_PER_VORONOI]; //(cpVect*)alloca(MAX_VERTEXES_PER_VORONOI * sizeof(cpVect));
int count = shape.Count; // cpPolyShapeGetCount();
count = (count > MAX_VERTEXES_PER_VORONOI ? MAX_VERTEXES_PER_VORONOI : count);
for (int i = 0; i < count; i++)
{
ping[i] = body.LocalToWorld(shape.GetVert(i));
}
cpPointQueryInfo info = null;
for (int i = 0; i < context.width; i++)
{
for (int j = 0; j < context.height; j++)
{
if (
!(i == cell_i && j == cell_j) &&
shape.PointQuery(cell, ref info) < 0
)
{
count = ClipCell(shape, cell, i, j, context, ping, pong, count);
for (int u = 0; u < pong.Length; u++)
{
if (pong[u] != null)
{
ping[u] = new cpVect(pong[u]);
}
}
}
}
}
cpVect centroid = cp.CentroidForPoly(count, ping);
float mass = cp.AreaForPoly(count, ping, 0) * DENSITY;
float moment = cp.MomentForPoly(mass, count, ping, cpVect.cpvneg(centroid), 0);
cpBody new_body = space.AddBody(new cpBody(mass, moment));
new_body.SetPosition(centroid);
new_body.SetPosition(centroid);
new_body.SetVelocity(body.GetVelocityAtLocalPoint(centroid));
new_body.SetAngularVelocity(body.GetAngularVelocity());
cpTransform transform = cpTransform.Translate(cpVect.cpvneg(centroid));
cpShape new_shape = space.AddShape(new cpPolyShape(new_body, count, ping, transform, 0));
// Copy whatever properties you have set on the original shape that are important
new_shape.SetFriction(shape.GetFriction());
}
public override float CalculateDefaultMoment()
{
cpPolyShape shape = (cpPolyShape)_info.GetShapes().FirstOrDefault();
return(_mass == cp.Infinity ? cp.Infinity
: cp.MomentForPoly(_mass, shape.Count, shape.GetVertices(), cpVect.Zero, 0.0f));
}
cpPolyShapeNearestPointQuery(cpPolyShape poly, cpVect p, cpNearestPointQueryInfo* info)
{
int count = poly.numVerts;
cpSplittingPlane planes = poly.tPlanes;
cpVect[] verts = poly.tVerts;
cpVect v0 = verts[count - 1];
double minDist = double.PositiveInfinity;
cpVect closestPoint = cpvzero;
bool outside = false;
for (int i = 0; i < count; i++)
{
if (cpSplittingPlaneCompare(planes[i], p) > 0.0f) outside = true;
cpVect v1 = verts[i];
cpVect closest = cpClosetPointOnSegment(p, v0, v1);
double dist = cpVect.Distance(p, closest);
if (dist < minDist)
{
minDist = dist;
closestPoint = closest;
}
v0 = v1;
}
info.shape = (cpShape)poly;
info.p = closestPoint; // TODO div/0
info.d = (outside ? minDist : -minDist);
}
public void ShatterShape(cpPolyShape shape, float cellSize, cpVect focus)
{
space.RemoveShape(shape);
space.RemoveBody(shape.GetBody());
cpBB bb = shape.bb;
int width = (int)((bb.r - bb.l) / cellSize) + 1;
int height = (int)((bb.t - bb.b) / cellSize) + 1;
// printf("Splitting as %dx%d\n", width, height);
WorleyContex context = new WorleyContex((int)RandomHelper.frand(), cellSize, width, height, bb, focus);
for (int i = 0; i < context.width; i++)
{
for (int j = 0; j < context.height; j++)
{
cpVect cell = WorleyPoint(i, j, ref context);
cpPointQueryInfo cp = null;
if (shape.PointQuery(cell, ref cp) < 0.0f)
{
ShatterCell(shape, cell, i, j, ref context);
}
}
}
//cpBodyFree(cpShapeGetBody(shape));
//cpShapeFree(shape);
}
//protected cpVect _offset;
#endregion
#region PUBLIC METHODS
public CCPhysicsShapeBox(CCSize size, CCPhysicsMaterial material, float radius)
{
cpVect wh = PhysicsHelper.size2cpv(size);
_type = PhysicsType.BOX;
cpVect[] vec =
{
new cpVect(-wh.x / 2.0f, -wh.y / 2.0f),
new cpVect(-wh.x / 2.0f, wh.y / 2.0f),
new cpVect(wh.x / 2.0f, wh.y / 2.0f),
new cpVect(wh.x / 2.0f, -wh.y / 2.0f)
};
cpShape shape = new cpPolyShape(CCPhysicsShapeInfo.SharedBody, 4, vec, radius);
_info.Add(shape);
//_offset = offset;
_area = CalculateArea();
_mass = material.density == cp.Infinity ? cp.Infinity : material.density * _area;
_moment = CalculateDefaultMoment();
Material = material;
}
public void Draw(cpPolyShape poly, cpColor color)
{
cpColor fill = new cpColor(color);
fill.a = cp.cpflerp(color.a, 1.0f, 0.5f);
DrawPolygon(poly.GetVertices(), poly.Count, color);
}
public void DrawShape(cpShape shape)
{
cpBody body = shape.body;
cpColor color = cp.GetShapeColor(shape);; // ColorForBody(body);
switch (shape.shapeType)
{
case cpShapeType.Circle:
{
cpCircleShape circle = (cpCircleShape)shape;
if ((Flags & cpDrawFlags.BB) == cpDrawFlags.BB || (Flags & cpDrawFlags.All) == cpDrawFlags.All)
{
Draw(circle.bb);
}
if ((Flags & cpDrawFlags.Shapes) == cpDrawFlags.Shapes || (Flags & cpDrawFlags.All) == cpDrawFlags.All)
{
Draw(circle, color);
}
}
break;
case cpShapeType.Segment:
{
cpSegmentShape seg = (cpSegmentShape)shape;
if ((Flags & cpDrawFlags.BB) == cpDrawFlags.BB || (Flags & cpDrawFlags.All) == cpDrawFlags.All)
{
Draw(seg.bb);
}
if ((Flags & cpDrawFlags.Shapes) == cpDrawFlags.Shapes || (Flags & cpDrawFlags.All) == cpDrawFlags.All)
{
Draw(seg, color);
}
}
break;
case cpShapeType.Polygon:
{
cpPolyShape poly = (cpPolyShape)shape;
if ((Flags & cpDrawFlags.BB) == cpDrawFlags.BB || (Flags & cpDrawFlags.All) == cpDrawFlags.All)
{
Draw(poly.bb);
}
if ((Flags & cpDrawFlags.Shapes) == cpDrawFlags.Shapes || (Flags & cpDrawFlags.All) == cpDrawFlags.All)
{
Draw(poly, color);
}
}
break;
default:
cp.AssertHard(false, "Bad assertion in DrawShape()");
break;
}
}
public CCSize GetSize()
{
cpPolyShape shape = (_info.GetShapes().FirstOrDefault() as cpPolyShape); //->getShapes().front();
return(PhysicsHelper.cpv2size(
new cpVect(
cpVect.cpvdist(shape.GetVert(1), shape.GetVert(2)),
cpVect.cpvdist(shape.GetVert(0), shape.GetVert(1)))));
}
cpPolyShapeTransformAxes(cpPolyShape poly, cpVect p, cpVect rot)
{
cpSplittingPlane src = poly.planes;
cpSplittingPlane dst = poly.tPlanes;
for (int i = 0; i < poly.numVerts; i++)
{
cpVect n = cpvrotate(src[i].n, rot);
dst[i].n = n;
dst[i].d = cpVect.Dot(p, n) + src[i].d;
}
}
public void ClipPoly(cpSpace space, cpShape shp, cpVect n, float dist)
{
cpPolyShape shape = (cpPolyShape)shp;
cpBody body = shape.GetBody();
int count = shape.Count;
int clippedCount = 0;
cpVect[] clipped = new cpVect[count + 1];
for (int i = 0, j = count - 1; i < count; j = i, i++)
{
cpVect a = body.LocalToWorld(shape.GetVert(j));
float a_dist = cpVect.cpvdot(a, n) - dist;
if (a_dist < 0)
{
clipped[clippedCount] = a;
clippedCount++;
}
cpVect b = body.LocalToWorld(shape.GetVert(i));
float b_dist = cpVect.cpvdot(b, n) - dist;
if (a_dist * b_dist < 0)
{
float t = cp.cpfabs(a_dist) / (cp.cpfabs(a_dist) + cp.cpfabs(b_dist));
clipped[clippedCount] = cpVect.cpvlerp(a, b, t);
clippedCount++;
}
}
cpVect centroid = cp.CentroidForPoly(clippedCount, clipped);
float mass = cp.AreaForPoly(clippedCount, clipped, 0) * DENSITY;
float moment = cp.MomentForPoly(mass, clippedCount, clipped, cpVect.cpvneg(centroid), 0);
cpBody new_body = space.AddBody(new cpBody(mass, moment));
new_body.SetPosition(centroid);
new_body.SetVelocity(body.GetVelocityAtWorldPoint(centroid));
new_body.SetAngularVelocity(body.GetAngularVelocity());
cpTransform transform = cpTransform.Translate(cpVect.cpvneg(centroid));
cpShape new_shape = space.AddShape(new cpPolyShape(new_body, clippedCount, clipped, transform, 0));
// Copy whatever properties you have set on the original shape that are important
new_shape.SetFriction(shape.GetFriction());
}
static public void add_box(cpSpace space, int index, float size)
{
float mass = size * size / 100.0f;
cpBody body = space.AddBody(new cpBody(mass, cp.MomentForBox(mass, size, size)));
// cpBody body = cpSpaceAddBody(space, cpBodyInit(&bodies[i], mass, cpMomentForBox(mass, size, size)));
body.SetPosition(cpVect.cpvmult(cp.frand_unit_circle(), 180.0f));
cpPolyShape shape = space.AddShape(cpPolyShape.BoxShape(body, size - bevel * 2f, size - bevel * 2f, 0f)) as cpPolyShape;
shape.SetRadius(bevel);
shape.SetElasticity(0.0f);
shape.SetFriction(0.9f);
}
public void Init(CCPoint[] vecs, int count, CCPhysicsMaterial material, float radius)
{
_type = PhysicsType.POLYGEN;
cpShape shape = new cpPolyShape(CCPhysicsShapeInfo.SharedBody, count, PhysicsHelper.CCPointsTocpVects(vecs), radius);
_info.Add(shape);
_area = CalculateArea();
_mass = material.density == cp.Infinity ? cp.Infinity : material.density * _area;
_moment = CalculateDefaultMoment();
Material = material;
}
protected override void AddedToScene()
{
base.AddedToScene();
//PositionX += (windowSize.Width - 640) * .5d; //new CCPoint(150, 150);
space.SetIterations(60);
space.SetGravity(new cpVect(0, -500));
space.SetSleepTimeThreshold(0.5f);
space.SetCollisionSlop(0.5f);
space.SetSleepTimeThreshold(0.5f);
this.addFloor();
this.addWalls();
float width = 50;
float height = 60;
float mass = width * height * 1f / 1000f;
var rock = space.AddBody(new cpBody(mass, cp.MomentForBox(mass, width, height)));
rock.SetPosition(new cpVect(200, 0));
rock.SetAngle(1);
cpPolyShape shape = space.AddShape(cpPolyShape.BoxShape(rock, width, height, 0.0f)) as cpPolyShape;
shape.SetFriction(0.3f);
shape.SetElasticity(0.3f);
//shape.SetFilter(NOT_GRABBABLE_FILTER); //The box cannot be dragg
for (var i = 1; i <= 6; i++)
{
float radius = 20f;
mass = 3;
var body = space.AddBody(new cpBody(mass, cp.MomentForCircle(mass, 0f, radius, cpVect.Zero)));
body.SetPosition(new cpVect(i, (2 * radius + 5) * 1));
cpCircleShape circle = space.AddShape(new cpCircleShape(body, radius, cpVect.Zero)) as cpCircleShape;
circle.SetElasticity(0.8f);
circle.SetFriction(1);
}
var ramp = space.AddShape(new cpSegmentShape(space.GetStaticBody(), new cpVect(0, 0), new cpVect(300, 200), 10));
ramp.SetElasticity(1f);
ramp.SetFriction(1f);
ramp.SetFilter(NOT_GRABBABLE_FILTER);
Schedule();
}
/// <summary>
/// Create a physics body and a box shape and adds it to the world
/// NOTE: Be sure to call activate on the body when you are ready for it to be simulated!
/// </summary>
/// <param name="world">The world to add it to</param>
/// <param name="position">The position of the center of the box</param>
/// <param name="rotation">The rotation of the box</param>
/// <param name="size">The size of the box</param>
/// <param name="bodyType">The body type of the physics body</param>
/// <param name="mass">The mass of the physics body</param>
/// <param name="moment">The moment of inertia for the physics body</param>
/// <returns></returns>
public static PhysicsObject CreateBox(World world, Vector2 position, float rotation, Vector2 size, cpBodyType bodyType = cpBodyType.DYNAMIC, float mass = 1, float moment = 1)
{
//Create body
cpBody body = CreateBody(world, bodyType, mass, moment);
body.SetPosition(new cpVect(position.X / PHYSICS_TRANSFORM_SCALE, position.Y / PHYSICS_TRANSFORM_SCALE));
body.SetAngle(rotation);
cpPolyShape box = cpPolyShape.BoxShape(body, size.X / PHYSICS_TRANSFORM_SCALE, size.Y / PHYSICS_TRANSFORM_SCALE, 0.1f);
Worlds[world].AddShape(box);
//Return the Physics body
return(new PhysicsObject()
{
Body = body, Shape = box
});
}
static public void add_hexagon(cpSpace space, int index, float radius)
{
cpVect[] hexagon = new cpVect[6];
for (int i = 0; i < 6; i++)
{
float angle = -(float)Math.PI * 2.0f * i / 6.0f;
hexagon[i] = cpVect.cpvmult(cpVect.cpv(cp.cpfcos(angle), cp.cpfsin(angle)), radius - bevel);
}
float mass = radius * radius;
cpBody body = space.AddBody(new cpBody(mass, cp.MomentForPoly(mass, 6, hexagon, cpVect.Zero, 0.0f)));
body.SetPosition(cpVect.cpvmult(cp.frand_unit_circle(), 180.0f));
cpPolyShape shape = space.AddShape(new cpPolyShape(body, 6, hexagon, cpTransform.Identity, bevel)) as cpPolyShape;
shape.SetElasticity(0.0f);
shape.SetFriction(0.9f);
}
cpPolyShapeTransformVerts(cpPolyShape poly, cpVect p, cpVect rot)
{
cpVect[] src = poly.verts;
cpVect[] dst = poly.tVerts;
double l = double.PositiveInfinity, r = double.NegativeInfinity;
double b = double.PositiveInfinity, t = double.NegativeInfinity;
for (int i = 0; i < poly.numVerts; i++)
{
cpVect v = cpVect.Add(p, cpvrotate(src[i], rot));
dst[i] = v;
l = System.Math.Min(l, v.x);
r = System.Math.Max(r, v.x);
b = System.Math.Min(b, v.y);
t = System.Math.Max(t, v.y);
}
return cpBBNew(l, b, r, t);
}
public static Edge SupportEdgeForPoly(cpPolyShape poly, cpVect n)
{
ulong count = (ulong)poly.Count;
ulong i1 = cpCollision.PolySupportPointIndex(poly.Count, poly.planes, n);
// TODO get rid of mod eventually, very expensive on ARM
ulong i0 = (ulong)((i1 - 1 + count) % count);
ulong i2 = (ulong)((i1 + 1) % count);
cpSplittingPlane[] planes = poly.planes;
ulong hashid = poly.hashid;
if (cpVect.cpvdot(n, planes[i1].n) > cpVect.cpvdot(n, planes[i2].n))
{
Edge edge = new Edge(
new EdgePoint(planes[i0].v0, cp.CP_HASH_PAIR(hashid, i0)),
new EdgePoint(planes[i1].v0, cp.CP_HASH_PAIR(hashid, i1)),
poly.r, poly.planes[i1].n);
return edge;
}
else
{
Edge edge = new Edge(
new EdgePoint(planes[i1].v0, cp.CP_HASH_PAIR(hashid, i1)),
new EdgePoint(planes[i2].v0, cp.CP_HASH_PAIR(hashid, i2)),
poly.r, poly.planes[i2].n);
return edge;
}
}
public static SupportPoint PolySupportPoint(cpPolyShape poly, cpVect n)
{
ulong i = PolySupportPointIndex(poly.Count, poly.planes, n);
return new SupportPoint(poly.planes[i].v0, i);
}
findPointsBehindSeg(cpContact arr, int *num, cpSegmentShape seg, cpPolyShape poly, double pDist, double coef)
{
double dta = cpVect.CrossProduct(seg.tn, seg.ta);
double dtb = cpVect.CrossProduct(seg.tn, seg.tb);
cpVect n = cpVect.Multiply(seg.tn, coef);
for(int i=0; i<poly.numVerts; i++){
cpVect v = poly.tVerts[i];
if(cpVect.Dot(v, n) < cpVect.Dot(seg.tn, seg.ta)*coef + seg.r){
double dt = cpVect.CrossProduct(seg.tn, v);
if(dta >= dt && dt >= dtb){
cpContactInit(nextContactPoint(arr, num), v, n, pDist, CP_HASH_PAIR(poly.shape.hashid, i));
}
}
}
}
findVerts(cpContact arr, cpPolyShape poly1, cpPolyShape poly2, cpVect n, double dist)
{
int num = 0;
for(int i=0; i<poly1.numVerts; i++){
cpVect v = poly1.tVerts[i];
if(cpPolyShapeContainsVert(poly2, v))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly1.shape.hashid, i));
}
for(int i=0; i<poly2.numVerts; i++){
cpVect v = poly2.tVerts[i];
if(cpPolyShapeContainsVert(poly1, v))
cpContactInit(nextContactPoint(arr, &num), v, n, dist, CP_HASH_PAIR(poly2.shape.hashid, i));
}
return (num ? num : findVertsFallback(arr, poly1, poly2, n, dist));
}
findMSA(cpPolyShape poly, cpSplittingPlane planes, int num, double min_out)
{
int min_index = 0;
double min = cpPolyShapeValueOnAxis(poly, planes.n, planes.d);
if(min > 0.0f) return -1;
for(int i=1; i<num; i++){
double dist = cpPolyShapeValueOnAxis(poly, planes[i].n, planes[i].d);
if(dist > 0.0f) {
return -1;
} else if(dist > min){
min = dist;
min_index = i;
}
}
(*min_out) = min;
return min_index;
}
cpPolyShapeSegmentQuery(cpPolyShape poly, cpVect a, cpVect b, cpSegmentQueryInfo* info)
{
cpSplittingPlane axes = poly.tPlanes;
cpVect[] verts = poly.tVerts;
int numVerts = poly.numVerts;
for (int i = 0; i < numVerts; i++)
{
cpVect n = axes[i].n;
double an = cpVect.Dot(a, n);
if (axes[i].d > an) continue;
double bn = cpVect.Dot(b, n);
double t = (axes[i].d - an) / (bn - an);
if (t < 0.0f || 1.0f < t) continue;
cpVect point = cpvlerp(a, b, t);
double dt = -cpVect.CrossProduct(n, point);
double dtMin = -cpVect.CrossProduct(n, verts[i]);
double dtMax = -cpVect.CrossProduct(n, verts[(i + 1) % numVerts]);
if (dtMin <= dt && dt <= dtMax)
{
info.shape = (cpShape)poly;
info.t = t;
info.n = n;
}
}
}
setUpVerts(cpPolyShape poly, int numVerts, cpVect[] verts, cpVect offset)
{
// Fail if the user attempts to pass a concave poly, or a bad winding.
// cpAssertHard(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding. Consider using cpConvexHull() or CP_CONVEX_HULL().");
poly.numVerts = numVerts;
poly.verts = new cpVect[2 * numVerts];
poly.planes = new cpSplittingPlane[2 * numVerts];
poly.tVerts = poly.verts + numVerts;
poly.tPlanes = poly.planes + numVerts;
for (int i = 0; i < numVerts; i++)
{
cpVect a = cpVect.Add(offset, verts[i]);
cpVect b = cpVect.Add(offset, verts[(i + 1) % numVerts]);
cpVect n = cpvnormalize(cpvperp(cpVect.Sub(b, a)));
poly.verts[i] = a;
poly.planes[i].n = n;
poly.planes[i].d = cpVect.Dot(n, a);
}
}
cpPolyShapeInit(cpPolyShape poly, cpBody body, int numVerts, cpVect[] verts, cpVect offset)
{
setUpVerts(poly, numVerts, verts, offset);
cpShapeInit((cpShape)poly, &polyClass, body);
return poly;
}
cpBoxShapeInit(cpPolyShape poly, cpBody body, double width, double height)
{
double hw = width / 2.0f;
double hh = height / 2.0f;
return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh));
}
cpBoxShapeInit2(cpPolyShape poly, cpBody body, cpBB box)
{
cpVect[] verts = new cpVect[] {
cpv(box.l, box.b),
cpv(box.l, box.t),
cpv(box.r, box.t),
cpv(box.r, box.b)
};
return cpPolyShapeInit(poly, body, 4, verts, cpvzero);
}
cpPolyShapeDestroy(cpPolyShape poly)
{
cpfree(poly.verts);
cpfree(poly.planes);
}
public bool waterPreSolve(cpArbiter arb, cpSpace space, object o)
{
cpShape obj1, obj2;
arb.GetShapes(out obj1, out obj2);
cpPolyShape water = obj1 as cpPolyShape;
cpPolyShape poly = obj2 as cpPolyShape;
cpBody body = poly.GetBody();
float level = water.GetBB().t; // cpShapeGetBB().t;
int count = poly.Count; //cpPolyShapeGetCount(poly.g);
int clippedCount = 0;
cpVect[] clipped = new cpVect[10];
for (int i = 0, j = count - 1; i < count; j = i, i++)
{
cpVect a = body.LocalToWorld(poly.GetVert(j));
cpVect b = body.LocalToWorld(poly.GetVert(i));
if (a.y < level)
{
clipped[clippedCount] = a;
clippedCount++;
}
float a_level = a.y - level;
float b_level = b.y - level;
if (a_level * b_level < 0.0f)
{
float t = cp.cpfabs(a_level) / (cp.cpfabs(a_level) + cp.cpfabs(b_level));
clipped[clippedCount] = cpVect.cpvlerp(a, b, t);
clippedCount++;
}
}
// Calculate buoyancy from the clipped polygon area
float clippedArea = cp.AreaForPoly(clippedCount, clipped, 0.0f);
float displacedMass = clippedArea * FLUID_DENSITY;
cpVect centroid = cp.CentroidForPoly(clippedCount, clipped);
//ChipmunkDebugDrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f));
//ChipmunkDebugDrawDot(5, centroid, RGBAColor(0, 0, 1, 1));
float dt = space.GetCurrentTimeStep();
cpVect g = space.GetGravity();
// Apply the buoyancy force as an impulse.
body.ApplyImpulseAtWorldPoint(cpVect.cpvmult(g, -displacedMass * dt), centroid);
// Apply linear damping for the fluid drag.
cpVect v_centroid = body.GetVelocityAtWorldPoint(centroid);
float k = k_scalar_body(body, centroid, cpVect.cpvnormalize(v_centroid));
float damping = clippedArea * FLUID_DRAG * FLUID_DENSITY;
float v_coef = cp.cpfexp(-damping * dt * k); // linear drag
// cpfloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag
body.ApplyImpulseAtWorldPoint(cpVect.cpvmult(cpVect.cpvsub(cpVect.cpvmult(v_centroid, v_coef), v_centroid), 1.0f / k), centroid);
// Apply angular damping for the fluid drag.
cpVect cog = body.LocalToWorld(body.GetCenterOfGravity());
float w_damping = cp.MomentForPoly(FLUID_DRAG * FLUID_DENSITY * clippedArea, clippedCount, clipped, cpVect.cpvneg(cog), 0.0f);
body.SetAngularVelocity(body.GetAngularVelocity() * cp.cpfexp(-w_damping * dt / body.GetMoment()));
return(true);
}
