static unsigned int _shape_add(Entity ent, PhysicsShape type, cpShape *shape)
{
PhysicsInfo *info;
ShapeInfo *shapeInfo;
info = entitypool_get(pool, ent);
error_assert(info);
/* init ShapeInfo */
shapeInfo = array_add(info->shapes);
shapeInfo->type = type;
shapeInfo->shape = shape;
/* init cpShape */
cpShapeSetBody(shape, info->body);
cpSpaceAddShape(space, shape);
cpShapeSetFriction(shapeInfo->shape, 1);
cpShapeSetUserData(shapeInfo->shape, ent);
/* update moment */
if (!cpBodyIsStatic(info->body))
{
if (array_length(info->shapes) > 1)
cpBodySetMoment(info->body, _moment(info->body, shapeInfo)
+ cpBodyGetMoment(info->body));
else
cpBodySetMoment(info->body, _moment(info->body, shapeInfo));
}
return array_length(info->shapes) - 1;
}
void update_friction(int n)
{
// kill lateral velocity
const cpFloat max_lateral_impulse = 300;
cpVect impulse = cpvmult(cpvneg(lateral_velocity(n)), cpBodyGetMass(tire[n]));
//printf("%f\n", cpvlength(impulse));
if(cpvlength(impulse) > max_lateral_impulse)
impulse = cpvmult(impulse, max_lateral_impulse / cpvlength(impulse));
cpBodyApplyImpulse(tire[n], impulse, cpvzero);
// TODO - kill angular velocity?
cpFloat inertia = cpBodyGetMoment(tire[n]);
cpFloat av = cpBodyGetAngVel(tire[n]);
if(av != 0)
cpBodySetAngVel(tire[n], av / 1.2);
// apply drag
cpVect forward_normal = forward_velocity(n);
cpFloat forward_speed = cpvlength(forward_normal);
if(forward_speed < 1) {
cpBodySetVel(tire[n], cpvzero);
} else {
forward_normal = cpvnormalize(forward_normal);
cpFloat drag = -1 * forward_speed;
cpBodyApplyImpulse(tire[n], cpvmult(forward_normal, drag), cpvzero);
}
}
bool physics_get_freeze_rotation(Entity ent)
{
PhysicsInfo *info = entitypool_get(pool, ent);
error_assert(info);
/* TODO: do this a better way? maybe store separate flag */
return cpBodyGetMoment(info->body) == SCALAR_INFINITY;
}
static int l_physics_getBodyMoment(lua_State* state)
{
l_tools_checkUserDataPlusErrMsg(state, 1, "You must provide a body");
l_physics_Body* body = (l_physics_Body*)lua_touserdata(state, 1);
lua_pushnumber(state, cpBodyGetMoment(body->body));
return 1;
}
static cpBody *utils_update_drawing(cpBody *drawing) {
cpFloat mass = cpBodyGetMass(drawing);
cpFloat moment = cpBodyGetMoment(drawing);
Body_data *pa = cpBodyGetUserData(drawing);
//cpFloat x = g_array_index(pa->x_values, cpFloat, 0);
//cpFloat y = g_array_index(pa->y_values, cpFloat, 0);
cpVect pos_a, pos_b;
cpVect origin = cpBodyGetPos(drawing);
cpFloat mi, micx = 0, micy = 0;
int length = pa->x_values->len;
for (int index = 1; index < length; index++) {
pos_a = cpv(g_array_index(pa->x_values, cpFloat, index - 1), g_array_index(pa->y_values, cpFloat, index - 1));
pos_b = cpv(g_array_index(pa->x_values, cpFloat, index), g_array_index(pa->y_values, cpFloat, index));
//fprintf(stdout, "Pos_a = (%5.2f, %5.2f)\n", pos_a.x, pos_a.y);
mi = (CRAYON_MASS * cpAreaForSegment( pos_a, pos_b, CRAYON_RADIUS ));
micx += mi * ((pos_a.x + pos_b.x) / 2);
micy += mi * ((pos_a.y + pos_b.y) / 2);
mass += mi;
moment += cpMomentForSegment(mass, pos_a, pos_b); // not actually sum, but maybe it is
}
cpBodySetMass(drawing, mass);
cpBodySetMoment(drawing, moment);
// center of mass is the average of all vertices NOT
//cpVect new_origin = cpv(x / length, y / length);
cpVect new_origin = cpv(micx / mass, micy / mass);
new_origin = cpBodyLocal2World(drawing, new_origin);
cpBodySetPos( drawing, new_origin );
//fprintf(stdout, "Position set at (%5.2f, %5.2f)\n", new_origin.x, new_origin.y);
cpSpace * space = cpBodyGetSpace(drawing);
cpSpaceReindexShapesForBody(space, drawing);
//cpBodySetPos(drawing, cpv(pos.x + (second.x / length), pos.y + (second.y / length)));
//pa->offset = cpvsub(new_origin, origin);
pa = shift_origin(drawing, origin, new_origin);
cpBodySetUserData(drawing, pa);
if (space)
post_step_body_replace_shapes(space, drawing, NULL);
else
fprintf(stderr, "WTF\n");
//if (!(cpSpaceAddPostStepCallback(space, (cpPostStepFunc) post_step_body_replace_shapes, drawing, NULL)))
//fprintf(stderr, "FAILED POST-STEP CALLBACK\n\n");
return drawing;
}
static cpBody *utils_update_drawing(cpBody *drawing) {
cpFloat mass = cpBodyGetMass(drawing);
cpFloat moment = cpBodyGetMoment(drawing);
Point_array *pa = cpBodyGetUserData(drawing);
cpFloat x = g_array_index(pa->x_values, cpFloat, 0);
cpFloat y = g_array_index(pa->y_values, cpFloat, 0);
cpVect pos_a, pos_b;
cpVect origin = cpBodyGetPos(drawing);
cpFloat mi, micx = 0, micy = 0;
int length = pa->x_values->len;
for (int index = 1; index < length; index++) {
pos_a = cpv(g_array_index(pa->x_values, cpFloat, index - 1), g_array_index(pa->y_values, cpFloat, index - 1));
pos_b = cpv(g_array_index(pa->x_values, cpFloat, index), g_array_index(pa->y_values, cpFloat, index));
cpvadd(pos_a, origin);
cpvadd(pos_b, origin);
x += pos_b.x;
y += pos_b.y;
mi = (CRAYON_MASS * cpAreaForSegment( pos_a, pos_b, CRAYON_RADIUS ));
micx += mi * ((pos_a.x + pos_b.x) / 2);
micy += mi * ((pos_a.y + pos_b.y) / 2);
mass += mi;
moment += cpMomentForSegment(mass, pos_a, pos_b); // not actually sum
}
cpBodySetMass(drawing, mass);
cpBodySetMoment(drawing, moment);
// center of mass is the average of all vertices NOT
//cpVect new_origin = cpv(x / length, y / length);
cpVect new_origin = cpv(micx / mass, micy / mass);
cpBodySetPos( drawing, new_origin );
//cpBodySetPos(drawing, cpv(pos.x + (second.x / length), pos.y + (second.y / length)));
pa = shift_origin(pa, origin, new_origin);
cpBodySetUserData(drawing, pa);
return drawing;
}
void RigidBody2D::SetGeom(Collider2DRef geom, bool recomputeMoment)
{
// We have no public way of getting rid of an existing geom without removing the whole body
// So let's save some attributes of the body, destroy it and rebuild it
if (m_geom)
{
cpFloat mass = cpBodyGetMass(m_handle);
cpFloat moment = cpBodyGetMoment(m_handle);
cpBody* newHandle = Create(static_cast<float>(mass), static_cast<float>(moment));
CopyBodyData(m_handle, newHandle);
Destroy();
m_handle = newHandle;
}
if (geom)
m_geom = geom;
else
m_geom = NullCollider2D::New();
m_geom->GenerateShapes(this, &m_shapes);
cpSpace* space = m_world->GetHandle();
for (cpShape* shape : m_shapes)
cpShapeSetUserData(shape, this);
if (m_isSimulationEnabled)
RegisterToSpace();
if (recomputeMoment)
{
if (!IsStatic() && !IsKinematic())
cpBodySetMoment(m_handle, m_geom->ComputeMomentOfInertia(m_mass));
}
}
cpFloat cBody::Moment() const {
return cpBodyGetMoment( mBody );
}
float RigidBody2D::GetMomentOfInertia() const
{
return float(cpBodyGetMoment(m_handle));
}
cpFloat Body::getMoment(void)
{
return cpBodyGetMoment(body);
}
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;
}
// Modified from chipmunk_private.h
cpFloat Buoyancy::KScalarBody(cpBody *body, cpVect point, cpVect n)
{
cpFloat rcn = cpvcross(cpvsub(point, cpBodyGetPosition(body)), n);
return 1.0f/cpBodyGetMass(body) + rcn*rcn/cpBodyGetMoment(body);
}
