static void
add_box(cpSpace *space)
{
const cpFloat size = 10.0f;
const cpFloat mass = 1.0f;
cpVect verts[] = {
cpv(-size,-size),
cpv(-size, size),
cpv( size, size),
cpv( size,-size),
};
cpFloat radius = cpvlength(cpv(size, size));
cpVect pos = rand_pos(radius);
cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForPoly(mass, 4, verts, cpvzero, 0.0f)));
body->velocity_func = planetGravityVelocityFunc;
cpBodySetPosition(body, pos);
// Set the box's velocity to put it into a circular orbit from its
// starting position.
cpFloat r = cpvlength(pos);
cpFloat v = cpfsqrt(gravityStrength / r) / r;
cpBodySetVelocity(body, cpvmult(cpvperp(pos), v));
// Set the box's angular velocity to match its orbital period and
// align its initial angle with its position.
cpBodySetAngularVelocity(body, v);
cpBodySetAngle(body, cpfatan2(pos.y, pos.x));
cpShape *shape = cpSpaceAddShape(space, cpPolyShapeNew(body, 4, verts, cpTransformIdentity, 0.0));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 0.7f);
}
void PhysicsForceField::addBodyField(cpVect p, PhysicsBody* body, float mass, cpVect& ret)
{
cpBody* cpBody = body->getCPBody();
cpVect d = cpvsub(cpBodyGetPosition(cpBody), p);
float dlensq = cpvlengthsq(d);
if (dlensq >= _minDistanceSq) {
ret = cpvadd(ret, cpvmult(d, mass / (dlensq*cpfsqrt(dlensq)) ));
}
}
static void
planetGravityVelocityFunc(cpBody *body, cpVect gravity, cpFloat damping, cpFloat dt)
{
// Gravitational acceleration is proportional to the inverse square of
// distance, and directed toward the origin. The central planet is assumed
// to be massive enough that it affects the satellites but not vice versa.
cpVect p = cpBodyGetPosition(body);
cpFloat sqdist = cpvlengthsq(p);
cpVect g = cpvmult(p, -gravityStrength / (sqdist * cpfsqrt(sqdist)));
cpBodyUpdateVelocity(body, g, damping, dt);
}
static void
cpCircleShapePointQuery(cpCircleShape *circle, cpVect p, cpPointQueryExtendedInfo *info){
cpVect delta = cpvsub(p, circle->tc);
cpFloat distsq = cpvlengthsq(delta);
cpFloat r = circle->r;
if(distsq < r*r){
info->shape = (cpShape *)circle;
cpFloat dist = cpfsqrt(distsq);
info->d = r - dist;
info->n = cpvmult(delta, 1.0/dist);
}
}
static void
make_leg(cpSpace *space, cpFloat side, cpFloat offset, cpBody *chassis, cpBody *crank, cpVect anchor)
{
cpVect a, b;
cpShape *shape;
cpFloat leg_mass = 1.0f;
// make leg
a = cpvzero, b = cpv(0.0f, side);
cpBody *upper_leg = cpSpaceAddBody(space, cpBodyNew(leg_mass, cpMomentForSegment(leg_mass, a, b, 0.0f)));
cpBodySetPosition(upper_leg, cpv(offset, 0.0f));
shape = cpSpaceAddShape(space, cpSegmentShapeNew(upper_leg, a, b, seg_radius));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
cpSpaceAddConstraint(space, cpPivotJointNew2(chassis, upper_leg, cpv(offset, 0.0f), cpvzero));
// lower leg
a = cpvzero, b = cpv(0.0f, -1.0f*side);
cpBody *lower_leg = cpSpaceAddBody(space, cpBodyNew(leg_mass, cpMomentForSegment(leg_mass, a, b, 0.0f)));
cpBodySetPosition(lower_leg, cpv(offset, -side));
shape = cpSpaceAddShape(space, cpSegmentShapeNew(lower_leg, a, b, seg_radius));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
shape = cpSpaceAddShape(space, cpCircleShapeNew(lower_leg, seg_radius*2.0f, b));
cpShapeSetFilter(shape, cpShapeFilterNew(1, CP_ALL_CATEGORIES, CP_ALL_CATEGORIES));
cpShapeSetElasticity(shape, 0.0f);
cpShapeSetFriction(shape, 1.0f);
cpSpaceAddConstraint(space, cpPinJointNew(chassis, lower_leg, cpv(offset, 0.0f), cpvzero));
cpSpaceAddConstraint(space, cpGearJointNew(upper_leg, lower_leg, 0.0f, 1.0f));
cpConstraint *constraint;
cpFloat diag = cpfsqrt(side*side + offset*offset);
constraint = cpSpaceAddConstraint(space, cpPinJointNew(crank, upper_leg, anchor, cpv(0.0f, side)));
cpPinJointSetDist(constraint, diag);
constraint = cpSpaceAddConstraint(space, cpPinJointNew(crank, lower_leg, anchor, cpvzero));
cpPinJointSetDist(constraint, diag);
}
static void
update(cpSpace *space, double dt)
{
int jumpState = (ChipmunkDemoKeyboard.y > 0.0f);
// If the jump key was just pressed this frame, jump!
if(jumpState && !lastJumpState && grounded){
cpFloat jump_v = cpfsqrt(2.0*JUMP_HEIGHT*GRAVITY);
playerBody->v = cpvadd(playerBody->v, cpv(0.0, jump_v));
remainingBoost = JUMP_BOOST_HEIGHT/jump_v;
}
// Step the space
cpSpaceStep(space, dt);
remainingBoost -= dt;
lastJumpState = jumpState;
}
// Add contact points for circle to circle collisions.
// Used by several collision tests.
static int
circle2circleQuery(const cpVect p1, const cpVect p2, const cpFloat r1, const cpFloat r2, cpContact *con)
{
cpFloat mindist = r1 + r2;
cpVect delta = cpvsub(p2, p1);
cpFloat distsq = cpvlengthsq(delta);
if(distsq >= mindist*mindist) return 0;
cpFloat dist = cpfsqrt(distsq);
// Allocate and initialize the contact.
cpContactInit(
con,
cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/(dist ? dist : INFINITY))),
(dist ? cpvmult(delta, 1.0f/dist) : cpv(1.0f, 0.0f)),
dist - mindist,
0
);
return 1;
}
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));
}
}
}
bool Player::update(float time, int direction) {
this->direction = direction;
loops++;
if(direction & UP){
jumpState = 1;
}else{
jumpState = 0;
}
if(jumpState && !lastJumpState && grounded){
cpFloat jump_v = cpfsqrt(2.0*JUMP_HEIGHT*GRAVITY);
playerBody->v = cpvadd(playerBody->v, cpv(0.0, -jump_v));
remainingBoost = JUMP_BOOST_HEIGHT/jump_v;
}
remainingBoost -= 1.0f/60.0f;
lastJumpState = jumpState;
return true;
}
// Add contact points for circle to circle collisions.
// Used by several collision tests.
static int
circle2circleQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con)
{
cpFloat mindist = r1 + r2;
cpVect delta = cpvsub(p2, p1);
cpFloat distsq = cpvlengthsq(delta);
if(distsq >= mindist*mindist) return 0;
cpFloat dist = cpfsqrt(distsq);
// To avoid singularities, do nothing in the case of dist = 0.
cpFloat non_zero_dist = (dist ? dist : INFINITY);
// Allocate and initialize the contact.
cpContactInit(
con,
cpvadd(p1, cpvmult(delta, 0.5f + (r1 - 0.5f*mindist)/non_zero_dist)),
cpvmult(delta, 1.0f/non_zero_dist),
dist - mindist,
0
);
return 1;
}
static void
cpSegmentShapePointQuery(cpSegmentShape *seg, cpVect p, cpPointQueryExtendedInfo *info){
if(!cpBBContainsVect(seg->shape.bb, p)) return;
cpVect a = seg->ta;
cpVect b = seg->tb;
cpVect seg_delta = cpvsub(b, a);
cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(p, a))/cpvlengthsq(seg_delta));
cpVect closest = cpvadd(a, cpvmult(seg_delta, closest_t));
cpVect delta = cpvsub(p, closest);
cpFloat distsq = cpvlengthsq(delta);
cpFloat r = seg->r;
if(distsq < r*r){
info->shape = (cpShape *)seg;
cpFloat dist = cpfsqrt(distsq);
info->d = r - dist;
info->n = cpvmult(delta, 1.0/dist);
}
}
cpFloat
cpvlength(const cpVect v)
{
return cpfsqrt(cpvdot(v, v));
}
