// set gravity (of previously chosen objects) to point towards black holes
void orbit(cpBody * body, cpVect gravity, cpFloat damping, cpFloat dt)
{
extern struct objnode objroot[];
struct objnode *objx;
struct cpBody *hole;
cpVect bpos, hpos, unitv, gvect;
cpFloat hmass, g, distsq;
objx = objroot;
while ((objx = objx->next) != NULL) {
if (objx->bhole == true && objx->b != body) {
hole = objx->b;
bpos = cpBodyGetPos(body);
hpos = cpBodyGetPos(hole);
distsq = cpvdistsq(hpos, bpos);
distsq = (distsq == 0) ? 1e-50 : distsq; // don't divide by zero
hmass = cpBodyGetMass(hole);
g = hmass * BGRAV * (1 / distsq);
g = (distsq < RDSQ) ? g * -REPFS : g; // shoot close balls away
unitv = cpvmult(cpvsub(hpos, bpos), (1 / sqrt(distsq)));
gvect = cpvmult(unitv, g);
cpBodyUpdateVelocity(body, gvect, damping, dt);
}
}
}
void MachineSystem::removePart(cpVect gridPosition)
{
int machineNum = machinePositionToNumber(gridPosition);
MachinePart *partToRemove = parts[machineNum];
if (partToRemove) {
cpBody *attachmentBody = partToRemove->getBody();
__block cpBody *pegBody = NULL;
cpBodyEachConstraint_b(attachmentBody, ^(cpConstraint *c) {
if (cpConstraintGetB(c) == attachmentBody) {
cpBody *otherBody = cpConstraintGetA(c); // the peg is always body A
if (cpBodyGetMass(otherBody) == INFINITY)
pegBody = otherBody;
}
});
assert(pegBody);
int nPegs = size.x * size.y;
// remove the attachments for this machine
for (int i=0; i<nPegs; i++) {
cpVect otherMachinePos = machineNumberToPosition(i);
detachMachines(gridPosition, otherMachinePos);
}
partToRemove->detachFromBody(pegBody);
cpBodyEachShape_b(pegBody, ^(cpShape *shape) {
cpSpaceRemoveStaticShape(space, shape);
cpShapeFree(shape);
});
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);
}
}
static int l_physics_getBodyMass(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, cpBodyGetMass(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;
}
void physics_add_top_down_friction(cpBody *body, cpBody *control, float friction, cpConstraint **out_pivot, cpConstraint **out_gear) {
//emulates linear friction
cpConstraint *pivot = cpSpaceAddConstraint(game.space, cpPivotJointNew2(
control,
body,
cpvzero,
cpvzero
));
cpConstraintSetErrorBias(pivot, cpBodyGetMass(body) * friction);
cpConstraintSetMaxForce(pivot, cpBodyGetMass(body) * friction);
//emulates angular friction
cpConstraint *gear = cpSpaceAddConstraint(game.space, cpGearJointNew(
control,
body,
0, 1
));
cpConstraintSetMaxBias(gear, 0);
cpConstraintSetMaxForce(gear, friction / cpBodyGetMass(body) / 10);
if (out_pivot) *out_pivot = pivot;
if (out_gear) *out_gear = gear;
}
void RigidBody2D::SetStatic(bool setStaticBody)
{
m_isStatic = setStaticBody;
m_world->RegisterPostStep(this, [](Nz::RigidBody2D* body)
{
if (body->IsStatic())
{
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_STATIC);
cpSpaceReindexShapesForBody(body->GetWorld()->GetHandle(), body->GetHandle());
}
else if (cpBodyGetMass(body->GetHandle()) > 0.f)
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_KINEMATIC);
else
cpBodySetType(body->GetHandle(), CP_BODY_TYPE_DYNAMIC);
});
}
/* 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 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;
}
int applykeys(Ship* s, bool* keylist)
{
if (keylist[0])
cpBodyApplyForceAtLocalPoint(s->body, cpv(0, 1e7), cpv(0.5, 0));
if (keylist[1])
cpBodyApplyForceAtLocalPoint(s->body, cpv(0, -1e6), cpv(0.5, 3));
if (keylist[2])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 0));
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 3));
}
if (keylist[3])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 3));
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 0));
}
if (keylist[4])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 3));
cpBodyApplyForceAtLocalPoint(s->body, cpv(-1e6, 0), cpv(1, 0));
}
if (keylist[5])
{
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 3));
cpBodyApplyForceAtLocalPoint(s->body, cpv(1e6, 0), cpv(0, 0));
}
if (keylist[6] && last_time_updated - s->last_fired > 1)
{
cpVect tip = cpBodyLocalToWorld(s->body, nosev + cpv(0, 0.1));
cpVect base = cpBodyLocalToWorld(s->body, cpv(0.5, 0));
cpVect newvel = cpvnormalize(tip - base) * shell_muzzle_vel + cpBodyGetVelocityAtLocalPoint(s->body, nosev);
Shell* newshell = addshell(tip, newvel, cpBodyGetAngle(s->body));
cpBodyApplyImpulseAtLocalPoint(s->body, cpv(0, -1)*cpBodyGetMass(newshell->body)*shell_muzzle_vel, cpv(0.5, 3));
s->last_fired = last_time_updated;
}
return 0;
}
/* 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;
}
}
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::Mass() const {
return cpBodyGetMass( mBody );
}
cpFloat Body::getMass(void)
{
return cpBodyGetMass(body);
}
// 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);
}
