inline cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t) { cpFloat omega = cpfacos(cpvdot(v1, v2)); if(omega){ cpFloat denom = 1.0f/cpfsin(omega); return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom)); } else { return v1; } }
inline cpVect cpvslerp(const cpVect v1, const cpVect v2, const cpFloat t) { cpFloat dot = cpvdot(cpvnormalize(v1), cpvnormalize(v2)); cpFloat omega = cpfacos(cpfclamp(dot, -1.0f, 1.0f)); if(omega == 0.0) { return v1; } else { cpFloat denom = 1.0f/cpfsin(omega); return cpvadd(cpvmult(v1, cpfsin((1.0f - t)*omega)*denom), cpvmult(v2, cpfsin(t*omega)*denom)); } }
cpVect cpvforangle(const cpFloat a) { return cpv(cpfcos(a), cpfsin(a)); }
static cpSpace * init(void) { staticBody = cpBodyNew(INFINITY, INFINITY); cpResetShapeIdCounter(); space = cpSpaceNew(); space->elasticIterations = 0; space->iterations = 5; cpSpaceResizeStaticHash(space, 40.0f, 999); cpSpaceResizeActiveHash(space, 30.0f, 2999); cpShape *shape; // add a non-collidable segment as a quick and dirty way to draw the query line shape = cpSegmentShapeNew(staticBody, cpvzero, cpv(100.0f, 0.0f), 4.0f); cpSpaceAddStaticShape(space, shape); shape->layers = 0; querySeg = shape; { // add a fat segment cpFloat mass = 1.0f; cpFloat length = 100.0f; cpVect a = cpv(-length/2.0f, 0.0f), b = cpv(length/2.0f, 0.0f); cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForSegment(mass, a, b))); body->p = cpv(0.0f, 100.0f); cpSpaceAddShape(space, cpSegmentShapeNew(body, a, b, 20.0f)); } { // add a static segment cpSpaceAddStaticShape(space, cpSegmentShapeNew(staticBody, cpv(0, 300), cpv(300, 0), 0.0f)); } { // add a pentagon cpFloat mass = 1.0f; const int NUM_VERTS = 5; cpVect verts[NUM_VERTS]; for(int i=0; i<NUM_VERTS; i++){ cpFloat angle = -2*(cpFloat)M_PI*i/((cpFloat) NUM_VERTS); verts[i] = cpv(30*cpfcos(angle), 30*cpfsin(angle)); } cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForPoly(mass, NUM_VERTS, verts, cpvzero))); body->p = cpv(50.0f, 50.0f); cpSpaceAddShape(space, cpPolyShapeNew(body, NUM_VERTS, verts, cpvzero)); } { // add a circle cpFloat mass = 1.0f; cpFloat r = 20.0f; cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, cpMomentForCircle(mass, 0.0f, r, cpvzero))); body->p = cpv(100.0f, 100.0f); cpSpaceAddShape(space, cpCircleShapeNew(body, r, cpvzero)); } return space; }