cpConstraint *
cpPivotJointNew(cpBody *a, cpBody *b, cpVect pivot)
{
cpVect anchorA = (a ? cpBodyWorldToLocal(a, pivot) : pivot);
cpVect anchorB = (b ? cpBodyWorldToLocal(b, pivot) : pivot);
return cpPivotJointNew2(a, b, anchorA, anchorB);
}
static cpBool
StickyPreSolve(cpArbiter *arb, cpSpace *space, void *data)
{
// We want to fudge the collisions a bit to allow shapes to overlap more.
// This simulates their squishy sticky surface, and more importantly
// keeps them from separating and destroying the joint.
// Track the deepest collision point and use that to determine if a rigid collision should occur.
cpFloat deepest = INFINITY;
// Grab the contact set and iterate over them.
cpContactPointSet contacts = cpArbiterGetContactPointSet(arb);
for(int i=0; i<contacts.count; i++){
// Sink the contact points into the surface of each shape.
contacts.points[i].point1 = cpvsub(contacts.points[i].point1, cpvmult(contacts.normal, STICK_SENSOR_THICKNESS));
contacts.points[i].point2 = cpvadd(contacts.points[i].point2, cpvmult(contacts.normal, STICK_SENSOR_THICKNESS));
deepest = cpfmin(deepest, contacts.points[i].distance);// + 2.0f*STICK_SENSOR_THICKNESS);
}
// Set the new contact point data.
cpArbiterSetContactPointSet(arb, &contacts);
// If the shapes are overlapping enough, then create a
// joint that sticks them together at the first contact point.
if(!cpArbiterGetUserData(arb) && deepest <= 0.0f){
CP_ARBITER_GET_BODIES(arb, bodyA, bodyB);
// Create a joint at the contact point to hold the body in place.
cpVect anchorA = cpBodyWorldToLocal(bodyA, contacts.points[0].point1);
cpVect anchorB = cpBodyWorldToLocal(bodyB, contacts.points[0].point2);
cpConstraint *joint = cpPivotJointNew2(bodyA, bodyB, anchorA, anchorB);
// Give it a finite force for the stickyness.
cpConstraintSetMaxForce(joint, 3e3);
// Schedule a post-step() callback to add the joint.
cpSpaceAddPostStepCallback(space, PostStepAddJoint, joint, NULL);
// Store the joint on the arbiter so we can remove it later.
cpArbiterSetUserData(arb, joint);
}
// Position correction and velocity are handled separately so changing
// the overlap distance alone won't prevent the collision from occuring.
// Explicitly the collision for this frame if the shapes don't overlap using the new distance.
return (deepest <= 0.0f);
// Lots more that you could improve upon here as well:
// * Modify the joint over time to make it plastic.
// * Modify the joint in the post-step to make it conditionally plastic (like clay).
// * Track a joint for the deepest contact point instead of the first.
// * Track a joint for each contact point. (more complicated since you only get one data pointer).
}
void RigidBody2D::SetMassCenter(const Vector2f& center, CoordSys coordSys)
{
cpVect massCenter = cpv(center.x, center.y);
switch (coordSys)
{
case CoordSys_Global:
massCenter = cpBodyWorldToLocal(m_handle, massCenter);
break;
case CoordSys_Local:
break; // Nothing to do
}
cpBodySetCenterOfGravity(m_handle, massCenter);
}
/* 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;
}
}
Vec2 PhysicsBody::world2Local(const Vec2& point)
{
return PhysicsHelper::cpv2point(cpBodyWorldToLocal(_cpBody, PhysicsHelper::point2cpv(point)));
}
static cpSpace *
init(void)
{
ChipmunkDemoMessageString = "Use the arrow keys to control the machine.";
cpSpace *space = cpSpaceNew();
cpSpaceSetGravity(space, cpv(0, -600));
cpBody *staticBody = cpSpaceGetStaticBody(space);
cpShape *shape;
// beveling all of the line segments slightly helps prevent things from getting stuck on cracks
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-256,16), cpv(-256,300), 2.0f));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-256,16), cpv(-192,0), 2.0f));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-192,0), cpv(-192, -64), 2.0f));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-128,-64), cpv(-128,144), 2.0f));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-192,80), cpv(-192,176), 2.0f));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-192,176), cpv(-128,240), 2.0f));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-128,144), cpv(192,64), 2.0f));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
cpVect verts[] = {
cpv(-30,-80),
cpv(-30, 80),
cpv( 30, 64),
cpv( 30,-80),
};
cpBody *plunger = cpSpaceAddBody(space, cpBodyNew(1.0f, INFINITY));
cpBodySetPosition(plunger, cpv(-160,-80));
shape = cpSpaceAddShape(space, cpPolyShapeNew(plunger, 4, verts, cpTransformIdentity, 0.0));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 0.5f);
cpShapeSetFilter(shape, cpShapeFilterNew(CP_NO_GROUP, 1, 1));
// add balls to hopper
for(int i=0; i<numBalls; i++)
balls[i] = add_ball(space, cpv(-224 + i,80 + 64*i));
// add small gear
cpBody *smallGear = cpSpaceAddBody(space, cpBodyNew(10.0f, cpMomentForCircle(10.0f, 80, 0, cpvzero)));
cpBodySetPosition(smallGear, cpv(-160,-160));
cpBodySetAngle(smallGear, -M_PI_2);
shape = cpSpaceAddShape(space, cpCircleShapeNew(smallGear, 80.0f, cpvzero));
cpShapeSetFilter(shape, CP_SHAPE_FILTER_NONE);
cpSpaceAddConstraint(space, cpPivotJointNew2(staticBody, smallGear, cpv(-160,-160), cpvzero));
// add big gear
cpBody *bigGear = cpSpaceAddBody(space, cpBodyNew(40.0f, cpMomentForCircle(40.0f, 160, 0, cpvzero)));
cpBodySetPosition(bigGear, cpv(80,-160));
cpBodySetAngle(bigGear, M_PI_2);
shape = cpSpaceAddShape(space, cpCircleShapeNew(bigGear, 160.0f, cpvzero));
cpShapeSetFilter(shape, CP_SHAPE_FILTER_NONE);
cpSpaceAddConstraint(space, cpPivotJointNew2(staticBody, bigGear, cpv(80,-160), cpvzero));
// connect the plunger to the small gear.
cpSpaceAddConstraint(space, cpPinJointNew(smallGear, plunger, cpv(80,0), cpv(0,0)));
// connect the gears.
cpSpaceAddConstraint(space, cpGearJointNew(smallGear, bigGear, -M_PI_2, -2.0f));
// feeder mechanism
cpFloat bottom = -300.0f;
cpFloat top = 32.0f;
cpBody *feeder = cpSpaceAddBody(space, cpBodyNew(1.0f, cpMomentForSegment(1.0f, cpv(-224.0f, bottom), cpv(-224.0f, top), 0.0f)));
cpBodySetPosition(feeder, cpv(-224, (bottom + top)/2.0f));
cpFloat len = top - bottom;
shape = cpSpaceAddShape(space, cpSegmentShapeNew(feeder, cpv(0.0f, len/2.0f), cpv(0.0f, -len/2.0f), 20.0f));
cpShapeSetFilter(shape, GRAB_FILTER);
cpSpaceAddConstraint(space, cpPivotJointNew2(staticBody, feeder, cpv(-224.0f, bottom), cpv(0.0f, -len/2.0f)));
cpVect anchr = cpBodyWorldToLocal(feeder, cpv(-224.0f, -160.0f));
cpSpaceAddConstraint(space, cpPinJointNew(feeder, smallGear, anchr, cpv(0.0f, 80.0f)));
// motorize the second gear
motor = cpSpaceAddConstraint(space, cpSimpleMotorNew(staticBody, bigGear, 3.0f));
return space;
}
