static cpBB
cpPolyShapeCacheData(cpShape *shape, cpVect p, cpVect rot)
{
cpPolyShape *poly = (cpPolyShape *)shape;
cpFloat l, b, r, t;
cpPolyShapeTransformAxes(poly, p, rot);
cpPolyShapeTransformVerts(poly, p, rot);
cpVect *verts = poly->tVerts;
l = r = verts[0].x;
b = t = verts[0].y;
// TODO do as part of cpPolyShapeTransformVerts?
for(int i=1; i<poly->numVerts; i++){
cpVect v = verts[i];
l = cpfmin(l, v.x);
r = cpfmax(r, v.x);
b = cpfmin(b, v.y);
t = cpfmax(t, v.y);
}
return cpBBNew(l, b, r, t);
}
static cpBB
cpCircleShapeCacheData(cpCircleShape *circle, cpVect p, cpVect rot)
{
cpVect c = circle->tc = cpvadd(p, cpvrotate(circle->c, rot));
cpFloat r = circle->r;
return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
}
void
cpSpaceHashRenderDebug(cpSpatialIndex *index)
{
if(index->klass != &klass){
cpAssertWarn(cpFalse, "Ignoring cpSpaceHashRenderDebug() call to non-spatial hash spatial index.");
return;
}
cpSpaceHash *hash = (cpSpaceHash *)index;
cpBB bb = cpBBNew(-320, -240, 320, 240);
cpFloat dim = hash->celldim;
int n = hash->numcells;
int l = (int)floor(bb.l/dim);
int r = (int)floor(bb.r/dim);
int b = (int)floor(bb.b/dim);
int t = (int)floor(bb.t/dim);
for(int i=l; i<=r; i++){
for(int j=b; j<=t; j++){
int cell_count = 0;
int index = hash_func(i,j,n);
for(cpSpaceHashBin *bin = hash->table[index]; bin; bin = bin->next)
cell_count++;
GLfloat v = 1.0f - (GLfloat)cell_count/10.0f;
glColor3f(v,v,v);
glRectf(i*dim, j*dim, (i + 1)*dim, (j + 1)*dim);
}
}
}
static cpBB
cpSegmentShapeCacheData(cpSegmentShape *seg, cpVect p, cpVect rot)
{
seg->ta = cpvadd(p, cpvrotate(seg->a, rot));
seg->tb = cpvadd(p, cpvrotate(seg->b, rot));
seg->tn = cpvrotate(seg->n, rot);
cpFloat l,r,b,t;
if(seg->ta.x < seg->tb.x){
l = seg->ta.x;
r = seg->tb.x;
} else {
l = seg->tb.x;
r = seg->ta.x;
}
if(seg->ta.y < seg->tb.y){
b = seg->ta.y;
t = seg->tb.y;
} else {
b = seg->tb.y;
t = seg->ta.y;
}
cpFloat rad = seg->r;
return cpBBNew(l - rad, b - rad, r + rad, t + rad);
}
static cpBB
cpPolyShapeCacheData(cpPolyShape *poly, cpTransform transform)
{
int count = poly->count;
struct cpSplittingPlane *dst = poly->planes;
struct cpSplittingPlane *src = dst + count;
cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
for(int i=0; i<count; i++){
cpVect v = cpTransformPoint(transform, src[i].v0);
cpVect n = cpTransformVect(transform, src[i].n);
dst[i].v0 = v;
dst[i].n = n;
l = cpfmin(l, v.x);
r = cpfmax(r, v.x);
b = cpfmin(b, v.y);
t = cpfmax(t, v.y);
}
cpFloat radius = poly->r;
return (poly->shape.bb = cpBBNew(l - radius, b - radius, r + radius, t + radius));
}
static cpBB
cpSegmentShapeCacheData(cpSegmentShape *seg, cpTransform transform)
{
seg->ta = cpTransformPoint(transform, seg->a);
seg->tb = cpTransformPoint(transform, seg->b);
seg->tn = cpTransformVect(transform, seg->n);
cpFloat l,r,b,t;
if(seg->ta.x < seg->tb.x){
l = seg->ta.x;
r = seg->tb.x;
} else {
l = seg->tb.x;
r = seg->ta.x;
}
if(seg->ta.y < seg->tb.y){
b = seg->ta.y;
t = seg->tb.y;
} else {
b = seg->tb.y;
t = seg->ta.y;
}
cpFloat rad = seg->r;
return cpBBNew(l - rad, b - rad, r + rad, t + rad);
}
cpPolyShape *
cpBoxShapeInit(cpPolyShape *poly, cpBody *body, cpFloat width, cpFloat height, cpFloat radius)
{
cpFloat hw = width/2.0f;
cpFloat hh = height/2.0f;
return cpBoxShapeInit2(poly, body, cpBBNew(-hw, -hh, hw, hh), radius);
}
static VALUE
rb_cpBBInitialize(int argc, VALUE *argv, VALUE self) {
VALUE l, b, r, t;
cpBB *bb = BBGET(self);
rb_scan_args(argc, argv, "04", &l, &b, &r, &t);
// initialize as a circle bounds box if ony 2 params
if (NIL_P(r)) {
if(NIL_P(l)) {
(*bb) = cpBBNew(0, 0, 1, 1); // unit box.
} else {
cpVect * p = VGET(l);
(*bb) = cpBBNewForCircle(*p, NUM2DBL(b));
}
} else {
(*bb) = cpBBNew(NUM2DBL(l), NUM2DBL(b), NUM2DBL(r), NUM2DBL(t));
}
return self;
}
static cpSpace *
init(void)
{
cpSpace *space = cpSpaceNew();
space->iterations = 10;
space->gravity = cpv(0, -GRAVITY);
// space->sleepTimeThreshold = 1000;
space->enableContactGraph = cpTrue;
cpBody *body, *staticBody = space->staticBody;
cpShape *shape;
// Create segments around the edge of the screen.
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,240), cpv(320,240), 0.0f));
shape->e = 1.0f; shape->u = 1.0f;
shape->layers = NOT_GRABABLE_MASK;
// Set up the player
body = cpSpaceAddBody(space, cpBodyNew(1.0f, INFINITY));
body->p = cpv(0, -200);
body->velocity_func = playerUpdateVelocity;
playerBody = body;
shape = cpSpaceAddShape(space, cpBoxShapeNew3(body, cpBBNew(-15.0, -27.5, 15.0, 27.5), 10.0));
// shape = cpSpaceAddShape(space, cpSegmentShapeNew(playerBody, cpvzero, cpv(0, radius), radius));
shape->e = 0.0f; shape->u = 0.0f;
shape->collision_type = 1;
playerShape = shape;
// Add some boxes to jump on
for(int i=0; i<6; i++){
for(int j=0; j<3; j++){
body = cpSpaceAddBody(space, cpBodyNew(4.0f, INFINITY));
body->p = cpv(100 + j*60, -200 + i*60);
shape = cpSpaceAddShape(space, cpBoxShapeNew(body, 50, 50));
shape->e = 0.0f; shape->u = 0.7f;
}
}
return space;
}
static void
cpSweep1DSegmentQuery(cpSweep1D *sweep, void *obj, cpVect a, cpVect b, cpFloat t_exit, cpSpatialIndexSegmentQueryFunc func, void *data)
{
cpBB bb = cpBBExpand(cpBBNew(a.x, a.y, a.x, a.y), b);
Bounds bounds = BBToBounds(sweep, bb);
TableCell *table = sweep->table;
for(int i=0, count=sweep->num; i<count; i++){
TableCell cell = table[i];
if(BoundsOverlap(bounds, cell.bounds)) func(obj, cell.obj, data);
}
}
void space_add_subscriber(erlmunk_space *s,
erlmunk_client *client,
ETERM *from,
float l, float b,
float r, float t) {
erlmunk_subscriber *subscriber = (erlmunk_subscriber *) malloc(sizeof(erlmunk_subscriber));
subscriber->id = s->subscriber_count++;
subscriber->bb = cpBBNew(l, b, r, t);
subscriber->client = client;
subscriber->from = from;
HASH_ADD_INT(s->subscribers, id, subscriber);
}
static inline cpBB
GetBB(cpBBTree *tree, void *obj)
{
cpBB bb = tree->spatialIndex.bbfunc(obj);
cpBBTreeVelocityFunc velocityFunc = tree->velocityFunc;
if(velocityFunc){
cpFloat coef = 0.1f;
cpFloat x = (bb.r - bb.l)*coef;
cpFloat y = (bb.t - bb.b)*coef;
cpVect v = cpvmult(velocityFunc(obj), 0.1f);
return cpBBNew(bb.l + cpfmin(-x, v.x), bb.b + cpfmin(-y, v.y), bb.r + cpfmax(x, v.x), bb.t + cpfmax(y, v.y));
} else {
return bb;
}
}
void init_physics()
{
// setup space
space = cpSpaceNew();
// setup tires
int i;
for(i=0; i<1; i++) {
cpFloat x = 0, y = 0, w = 0.5, h = 1.75;
cpFloat mass = 50;
cpFloat moment = cpMomentForBox(mass, w, h);
tire[i] = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpSpaceAddShape(space, cpBoxShapeNew2(tire[i],
cpBBNew(x-(w/2), y-(h/2), x+(w/2), y+(h/2))));
cpSpaceReindexShapesForBody(space, tire[0]);
cpBodySetAngle(tire[i], -M_PI/6);
//cpBodySetVel(tire[i], cpv(0, 10));
//cpBodyApplyImpulse(tire[i], cpv(0, 1000), cpvzero);
}
}
static cpBB
cpPolyShapeTransformVerts(cpPolyShape *poly, cpVect p, cpVect rot)
{
cpVect *src = poly->verts;
cpVect *dst = poly->tVerts;
cpFloat l = (cpFloat)INFINITY, r = -(cpFloat)INFINITY;
cpFloat b = (cpFloat)INFINITY, t = -(cpFloat)INFINITY;
for(int i=0; i<poly->numVerts; i++) {
cpVect v = cpvadd(p, cpvrotate(src[i], rot));
dst[i] = v;
l = cpfmin(l, v.x);
r = cpfmax(r, v.x);
b = cpfmin(b, v.y);
t = cpfmax(t, v.y);
}
return cpBBNew(l, b, r, t);
}
cpSpace *Buoyancy::Init()
{
ChipmunkDemo::Init();
space = cpSpaceNew();
cpSpaceSetIterations(space, 30);
cpSpaceSetGravity(space, cpv(0, -500));
// cpSpaceSetDamping(space, 0.5);
cpSpaceSetSleepTimeThreshold(space, 0.5f);
cpSpaceSetCollisionSlop(space, 0.5f);
cpBody *body, *staticBody = cpSpaceGetStaticBody(space);
cpShape *shape;
// Create segments around the edge of the screen.
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(-320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(320,-240), cpv(320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,-240), cpv(320,-240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(-320,240), cpv(320,240), 0.0f));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
{
// Add the edges of the bucket
cpBB bb = cpBBNew(-300, -200, 100, 0);
cpFloat radius = 5.0f;
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(bb.l, bb.b), cpv(bb.l, bb.t), radius));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(bb.r, bb.b), cpv(bb.r, bb.t), radius));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
shape = cpSpaceAddShape(space, cpSegmentShapeNew(staticBody, cpv(bb.l, bb.b), cpv(bb.r, bb.b), radius));
cpShapeSetElasticity(shape, 1.0f);
cpShapeSetFriction(shape, 1.0f);
cpShapeSetFilter(shape, NOT_GRABBABLE_FILTER);
// Add the sensor for the water.
shape = cpSpaceAddShape(space, cpBoxShapeNew2(staticBody, bb, 0.0));
cpShapeSetSensor(shape, cpTrue);
cpShapeSetCollisionType(shape, 1);
}
{
cpFloat width = 200.0f;
cpFloat height = 50.0f;
cpFloat mass = 0.3*FLUID_DENSITY*width*height;
cpFloat moment = cpMomentForBox(mass, width, height);
body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpBodySetPosition(body, cpv(-50, -100));
cpBodySetVelocity(body, cpv(0, -100));
cpBodySetAngularVelocity(body, 1);
shape = cpSpaceAddShape(space, cpBoxShapeNew(body, width, height, 0.0));
cpShapeSetFriction(shape, 0.8f);
}
{
cpFloat width = 40.0f;
cpFloat height = width*2;
cpFloat mass = 0.3*FLUID_DENSITY*width*height;
cpFloat moment = cpMomentForBox(mass, width, height);
body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
cpBodySetPosition(body, cpv(-200, -50));
cpBodySetVelocity(body, cpv(0, -100));
cpBodySetAngularVelocity(body, 1);
shape = cpSpaceAddShape(space, cpBoxShapeNew(body, width, height, 0.0));
cpShapeSetFriction(shape, 0.8f);
}
cpCollisionHandler *handler = cpSpaceAddCollisionHandler(space, 1, 0);
handler->preSolveFunc = (cpCollisionPreSolveFunc)WaterPreSolve;
handler->userData = this;
return space;
}
unsigned int physics_shape_add_box(Entity ent, BBox b, Scalar r)
{
cpShape *shape = cpBoxShapeNew3(NULL, cpBBNew(b.min.x, b.min.y,
b.max.x, b.max.y), r);
return _shape_add(ent, PS_POLYGON, shape);
}
static void
cpBBTreePointQuery(cpBBTree *tree, cpVect point, cpSpatialIndexQueryFunc func, void *data)
{
Node *root = tree->root;
if(root) SubtreeQuery(root, &point, cpBBNew(point.x, point.y, point.x, point.y), func, data);
}
static inline cpBB
bbFromCircle(const cpVect c, const cpFloat r)
{
return cpBBNew(c.x-r, c.y-r, c.x+r, c.y+r);
}
