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;
}
