void
cpArbiterApplyCachedImpulse(cpArbiter *arb, cpFloat dt_coef)
{
if(cpArbiterIsFirstContact(arb)) return;
cpBody *a = arb->body_a;
cpBody *b = arb->body_b;
for(int i=0; i<arb->numContacts; i++){
cpContact *con = &arb->contacts[i];
cpVect j = cpvrotate(con->n, cpv(con->jnAcc, con->jtAcc));
apply_impulses(a, b, con->r1, con->r2, cpvmult(j, dt_coef));
}
}
// takes array of vertices, returns center x and y coordinates in address of x and y
// algorithm: take average x value average y value
static void core_freestyle_center ( cpVect * verts, const int num_verts, cpVect * center ) {
cpFloat tot_length = 0.0;
// assume uniform mass, so length can be substitued as mass
cpVect center_of_mass = cpvzero;
for ( int i = 0; i< num_verts - 1; i++ ) {
cpFloat length = cpvdist ( verts[i], verts[i+1] );
cpVect to_cent = cpvmult ( cpvsub ( verts[i+1], verts[i] ), 0.5 );
cpVect loc_center = cpvadd ( verts[i], to_cent );
center_of_mass = cpvadd ( center_of_mass, cpvmult ( loc_center, length ) );
tot_length += length;
}
center_of_mass = cpvmult ( center_of_mass, 1/tot_length );
center->x = center_of_mass.x;
center->y = center_of_mass.y;
return;
}
// Recursive implementatino of the GJK loop.
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS) {
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
// TODO: should this be an area2x check?
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f) {
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
) {
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
return EPA(ctx, v0, p, v1);
} else {
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))) {
// The edge v0, v1 that we already have is the closest to (0, 0) since p was not closer.
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
// p was closer to the origin than our existing edge.
// Need to figure out which existing point to drop.
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)) {
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
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);
}
}
// 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
preStep(cpDampedSpring *spring, cpFloat dt, cpFloat dt_inv)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
// calculate mass normal
spring->nMass = 1.0f/k_scalar(a, b, spring->r1, spring->r2, spring->n);
spring->dt = dt;
spring->target_vrn = 0.0f;
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
static void
update(int ticks)
{
static int lastJumpState = 0;
int jumpState = (arrowDirection.y > 0.0f);
cpVect groundNormal = playerInstance.groundNormal;
if(groundNormal.y > 0.0f){
playerInstance.shape->surface_v = cpvmult(cpvperp(groundNormal), 400.0f*arrowDirection.x);
} else {
playerInstance.shape->surface_v = cpvzero;
}
cpBody *body = playerInstance.shape->body;
// apply jump
if(jumpState && !lastJumpState && cpvlengthsq(groundNormal)){
// body->v = cpvmult(cpvslerp(groundNormal, cpv(0.0f, 1.0f), 0.5f), 500.0f);
body->v = cpvadd(body->v, cpvmult(cpvslerp(groundNormal, cpv(0.0f, 1.0f), 0.75f), 500.0f));
}
if(playerInstance.groundShapes->num == 0){
cpFloat air_accel = body->v.x + arrowDirection.x*(2000.0f);
body->f.x = body->m*air_accel;
// body->v.x = cpflerpconst(body->v.x, 400.0f*arrowDirection.x, 2000.0f/60.0f);
}
int steps = 3;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
playerInstance.groundNormal = cpvzero;
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
}
lastJumpState = jumpState;
}
// Collide circles to segment shapes.
static int
circle2segment(cpShape *circleShape, cpShape *segmentShape, cpContact **con)
{
cpCircleShape *circ = (cpCircleShape *)circleShape;
cpSegmentShape *seg = (cpSegmentShape *)segmentShape;
// Radius sum
cpFloat rsum = circ->r + seg->r;
// Calculate normal distance from segment.
cpFloat dn = cpvdot(seg->tn, circ->tc) - cpvdot(seg->ta, seg->tn);
cpFloat dist = cpfabs(dn) - rsum;
if(dist > 0.0f) return 0;
// Calculate tangential distance along segment.
cpFloat dt = -cpvcross(seg->tn, circ->tc);
cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
cpFloat dtMax = -cpvcross(seg->tn, seg->tb);
// Decision tree to decide which feature of the segment to collide with.
if(dt < dtMin){
if(dt < (dtMin - rsum)){
return 0;
} else {
return circle2circleQuery(circ->tc, seg->ta, circ->r, seg->r, con);
}
} else {
if(dt < dtMax){
cpVect n = (dn < 0.0f) ? seg->tn : cpvneg(seg->tn);
(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
cpContactInit(
(*con),
cpvadd(circ->tc, cpvmult(n, circ->r + dist*0.5f)),
n,
dist,
0
);
return 1;
} else {
if(dt < (dtMax + rsum)) {
return circle2circleQuery(circ->tc, seg->tb, circ->r, seg->r, con);
} else {
return 0;
}
}
}
return 1;
}
static void
preStep(cpDampedSpring *spring, cpFloat dt)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
spring->r1 = cpvrotate(spring->anchr1, a->rot);
spring->r2 = cpvrotate(spring->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, spring->r2), cpvadd(a->p, spring->r1));
cpFloat dist = cpvlength(delta);
spring->n = cpvmult(delta, 1.0f/(dist ? dist : INFINITY));
cpFloat k = k_scalar(a, b, spring->r1, spring->r2, spring->n);
cpAssertSoft(k != 0.0, "Unsolvable spring.");
spring->nMass = 1.0f/k;
spring->target_vrn = 0.0f;
spring->v_coef = 1.0f - cpfexp(-spring->damping*dt*k);
// apply spring force
cpFloat f_spring = spring->springForceFunc((cpConstraint *)spring, dist);
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, f_spring*dt));
}
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);
}
}
static cpBody *
add_bar(cpVect a, cpVect b, int group)
{
cpVect center = cpvmult(cpvadd(a, b), 1.0f/2.0f);
cpFloat length = cpvlength(cpvsub(b, a));
cpFloat mass = length/160.0f;
cpBody *body = cpSpaceAddBody(space, cpBodyNew(mass, mass*length*length/12.0f));
body->p = center;
cpShape *shape = cpSpaceAddShape(space, cpSegmentShapeNew(body, cpvsub(a, center), cpvsub(b, center), 10.0f));
shape->group = group;
return body;
}
static void
preStep(cpPivotJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpTransformVect(a->transform, cpvsub(joint->anchorA, a->cog));
joint->r2 = cpTransformVect(b->transform, cpvsub(joint->anchorB, b->cog));
// Calculate mass tensor
joint-> k = k_tensor(a, b, joint->r1, joint->r2);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
static inline struct ClosestPoints
GJKRecurse(const struct SupportContext *ctx, const struct MinkowskiPoint v0, const struct MinkowskiPoint v1, const int iteration)
{
if(iteration > MAX_GJK_ITERATIONS){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
}
cpVect delta = cpvsub(v1.ab, v0.ab);
if(cpvcross(delta, cpvadd(v0.ab, v1.ab)) > 0.0f){
// Origin is behind axis. Flip and try again.
return GJKRecurse(ctx, v1, v0, iteration + 1);
} else {
cpFloat t = ClosestT(v0.ab, v1.ab);
cpVect n = (-1.0f < t && t < 1.0f ? cpvperp(delta) : cpvneg(LerpT(v0.ab, v1.ab, t)));
struct MinkowskiPoint p = Support(ctx, n);
#if DRAW_GJK
ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 1, 1, 1));
cpVect c = cpvlerp(v0.ab, v1.ab, 0.5);
ChipmunkDebugDrawSegment(c, cpvadd(c, cpvmult(cpvnormalize(n), 5.0)), RGBAColor(1, 0, 0, 1));
ChipmunkDebugDrawDot(5.0, p.ab, LAColor(1, 1));
#endif
if(
cpvcross(cpvsub(v1.ab, p.ab), cpvadd(v1.ab, p.ab)) > 0.0f &&
cpvcross(cpvsub(v0.ab, p.ab), cpvadd(v0.ab, p.ab)) < 0.0f
){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK->EPA iterations: %d", iteration);
// The triangle v0, p, v1 contains the origin. Use EPA to find the MSA.
return EPA(ctx, v0, p, v1);
} else {
// The new point must be farther along the normal than the existing points.
if(cpvdot(p.ab, n) <= cpfmax(cpvdot(v0.ab, n), cpvdot(v1.ab, n))){
cpAssertWarn(iteration < WARN_GJK_ITERATIONS, "High GJK iterations: %d", iteration);
return ClosestPointsNew(v0, v1);
} else {
if(ClosestDist(v0.ab, p.ab) < ClosestDist(p.ab, v1.ab)){
return GJKRecurse(ctx, v0, p, iteration + 1);
} else {
return GJKRecurse(ctx, p, v1, iteration + 1);
}
}
}
}
}
// Identify vertexes that have penetrated the segment.
static inline void
findPointsBehindSeg(cpContact **arr, int *max, int *num, cpSegmentShape *seg, cpPolyShape *poly, cpFloat pDist, cpFloat coef)
{
cpFloat dta = cpvcross(seg->tn, seg->ta);
cpFloat dtb = cpvcross(seg->tn, seg->tb);
cpVect n = cpvmult(seg->tn, coef);
for(int i=0; i<poly->numVerts; i++){
cpVect v = poly->tVerts[i];
if(cpvdot(v, n) < cpvdot(seg->tn, seg->ta)*coef + seg->r){
cpFloat dt = cpvcross(seg->tn, v);
if(dta >= dt && dt >= dtb){
cpContactInit(addContactPoint(arr, max, num), v, n, pDist, CP_HASH_PAIR(poly->shape.hashid, i));
}
}
}
}
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;
}
}
static void
applyImpulse(cpPinJoint *joint)
{
CONSTRAINT_BEGIN(joint, a, b);
cpVect n = joint->n;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, joint->r1, joint->r2, n);
// compute normal impulse
cpFloat jn = (joint->bias - vrn)*joint->nMass;
cpFloat jnOld = joint->jnAcc;
joint->jnAcc = cpfclamp(jnOld + jn, -joint->jnMax, joint->jnMax);
jn = joint->jnAcc - jnOld;
// apply impulse
apply_impulses(a, b, joint->r1, joint->r2, cpvmult(n, jn));
}
// This one is less gross, but still gross.
// TODO: Comment me!
static int
circle2poly(cpShape *shape1, cpShape *shape2, cpContact **con)
{
cpCircleShape *circ = (cpCircleShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
cpPolyShapeAxis *axes = poly->tAxes;
int mini = 0;
cpFloat min = cpvdot(axes->n, circ->tc) - axes->d - circ->r;
for(int i=0; i<poly->numVerts; i++){
cpFloat dist = cpvdot(axes[i].n, circ->tc) - axes[i].d - circ->r;
if(dist > 0.0f){
return 0;
} else if(dist > min) {
min = dist;
mini = i;
}
}
cpVect n = axes[mini].n;
cpVect a = poly->tVerts[mini];
cpVect b = poly->tVerts[(mini + 1)%poly->numVerts];
cpFloat dta = cpvcross(n, a);
cpFloat dtb = cpvcross(n, b);
cpFloat dt = cpvcross(n, circ->tc);
if(dt < dtb){
return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con);
} else if(dt < dta) {
(*con) = (cpContact *)cpmalloc(sizeof(cpContact));
cpContactInit(
(*con),
cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)),
cpvneg(n),
min,
0
);
return 1;
} else {
return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con);
}
}
static void
display(void)
{
glClear(GL_COLOR_BUFFER_BIT);
drawSpace(space, currDemo->drawOptions ? currDemo->drawOptions : &options);
drawInstructions();
drawInfo();
drawString(-300, -210, messageString);
glutSwapBuffers();
ticks++;
cpVect newPoint = cpvlerp(mousePoint_last, mousePoint, 0.25f);
mouseBody->p = newPoint;
mouseBody->v = cpvmult(cpvsub(newPoint, mousePoint_last), 60.0f);
mousePoint_last = newPoint;
currDemo->updateFunc(ticks);
}
static void
applyImpulse(cpDampedSpring *spring)
{
CONSTRAINT_BEGIN(spring, a, b);
cpVect n = spring->n;
cpVect r1 = spring->r1;
cpVect r2 = spring->r2;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
// compute velocity loss from drag
// not 100% certain this is derived correctly, though it makes sense
cpFloat v_damp = -vrn*spring->v_coef;
spring->target_vrn = vrn + v_damp;
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
void Bomb::explode(int timestep) {
exploding = true;
cpVect blast_dir;
for (int j=0; j < PARTICLE_TOTAL; j++) {
if (particles[j]->dead) {
blast_dir = cpvmult(cpvforangle(DEG2RAD(j*3)), (rand()%100)+40);
particles[j]->x = x;
particles[j]->y = y;
particles[j]->x_speed = blast_dir.x;
particles[j]->y_speed = blast_dir.y;
particles[j]->ttl = 1250;
particles[j]->birth = timestep;
particles[j]->color = 100;
particles[j]->dead = false;
}
}
}
static void
preStep(cpPivotJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
// Calculate mass tensor
k_tensor(a, b, joint->r1, joint->r2, &joint->k1, &joint->k2);
// compute max impulse
joint->jMaxLen = J_MAX(joint, dt);
// calculate bias velocity
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
joint->bias = cpvclamp(cpvmult(delta, -bias_coef(joint->constraint.errorBias, dt)/dt), joint->constraint.maxBias);
}
static void
applyImpulse(cpDampedSpring *spring)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
cpVect n = spring->n;
cpVect r1 = spring->r1;
cpVect r2 = spring->r2;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n);
// compute velocity loss from drag
cpFloat v_damp = (spring->target_vrn - vrn)*spring->v_coef;
spring->target_vrn = vrn + v_damp;
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
// This one is less gross, but still gross.
// TODO: Comment me!
static int
circle2poly(const cpShape *shape1, const cpShape *shape2, cpContact *con)
{
cpCircleShape *circ = (cpCircleShape *)shape1;
cpPolyShape *poly = (cpPolyShape *)shape2;
cpSplittingPlane *planes = poly->tPlanes;
int mini = 0;
cpFloat min = cpSplittingPlaneCompare(planes[0], circ->tc) - circ->r;
for(int i=0; i<poly->numVerts; i++){
cpFloat dist = cpSplittingPlaneCompare(planes[i], circ->tc) - circ->r;
if(dist > 0.0f){
return 0;
} else if(dist > min) {
min = dist;
mini = i;
}
}
cpVect n = planes[mini].n;
cpVect a = poly->tVerts[mini];
cpVect b = poly->tVerts[(mini + 1)%poly->numVerts];
cpFloat dta = cpvcross(n, a);
cpFloat dtb = cpvcross(n, b);
cpFloat dt = cpvcross(n, circ->tc);
if(dt < dtb){
return circle2circleQuery(circ->tc, b, circ->r, 0.0f, con);
} else if(dt < dta) {
cpContactInit(
con,
cpvsub(circ->tc, cpvmult(n, circ->r + min/2.0f)),
cpvneg(n),
min,
0
);
return 1;
} else {
return circle2circleQuery(circ->tc, a, circ->r, 0.0f, con);
}
}
static void
display(void)
{
PrintStringBuffer[0] = 0;
PrintStringCursor = PrintStringBuffer;
glMatrixMode(GL_MODELVIEW);
glLoadIdentity();
glScalef(scale, scale, 1.0);
glTranslatef(translate_x, translate_y, 0.0);
demos[demoIndex].drawFunc();
if(!paused || step){
cpVect newPoint = cpvlerp(mouseBody->p, ChipmunkDemoMouse, 0.25f);
mouseBody->v = cpvmult(cpvsub(newPoint, mouseBody->p), 60.0f);
mouseBody->p = newPoint;
demos[demoIndex].updateFunc(ticks);
ticks++;
ChipmunkDemoTime = ticks/60.0;
step = cpFalse;
}
if(drawBBs) cpSpaceEachShape(space, drawShapeBB, NULL);
glMatrixMode(GL_MODELVIEW);
glPushMatrix(); {
// Draw the text at fixed positions,
// but save the drawing matrix for the mouse picking
glLoadIdentity();
drawInstructions();
drawInfo();
drawString(-300, -200, ChipmunkDemoMessageString);
} glPopMatrix();
glutSwapBuffers();
glClear(GL_COLOR_BUFFER_BIT);
}
static void
preStep(cpPinJoint *joint, cpFloat dt)
{
cpBody *a = joint->constraint.a;
cpBody *b = joint->constraint.b;
joint->r1 = cpvrotate(joint->anchr1, a->rot);
joint->r2 = cpvrotate(joint->anchr2, b->rot);
cpVect delta = cpvsub(cpvadd(b->p, joint->r2), cpvadd(a->p, joint->r1));
cpFloat dist = cpvlength(delta);
joint->n = cpvmult(delta, 1.0f/(dist ? dist : (cpFloat)INFINITY));
// calculate mass normal
joint->nMass = 1.0f/k_scalar(a, b, joint->r1, joint->r2, joint->n);
// calculate bias velocity
cpFloat maxBias = joint->constraint.maxBias;
joint->bias = cpfclamp(-bias_coef(joint->constraint.errorBias, dt)*(dist - joint->dist)/dt, -maxBias, maxBias);
}
// determines moment of freestyle shape
// dos this by determining individual moments of segments,
//offsetting by distance to center squared
static double core_freestyle_moment ( cpVect * verts, const int num_verts, cpVect center, const double density ) {
double moment = 0;
for ( int i = 0; i < num_verts - 1; i++ ) {
cpFloat seg_length = cpvdist ( verts[i], verts[i + 1] );
double seg_mass = seg_length * density;
double moment_seg = seg_mass * seg_length * seg_length / 12.0; // moment around center of segment
// moment around center of object
cpVect seg_center = cpvadd ( verts[i], cpvmult ( cpvsub ( verts[i + 1], verts[i] ) ,seg_length / 2.0 ) ); // center of mass of segment
cpFloat seg_disp = cpvdist ( seg_center, center ); // distance of center of mass from center
double moment_disp = seg_mass * seg_disp * seg_disp;
moment += moment_seg + moment_disp; // add the two moments to total moment
}
return moment;
}
static void
applyImpulse(cpDampedSpring *spring)
{
cpBody *a = spring->constraint.a;
cpBody *b = spring->constraint.b;
cpVect n = spring->n;
cpVect r1 = spring->r1;
cpVect r2 = spring->r2;
// compute relative velocity
cpFloat vrn = normal_relative_velocity(a, b, r1, r2, n) - spring->target_vrn;
// compute velocity loss from drag
// not 100% certain this is derived correctly, though it makes sense
cpFloat v_damp = -vrn*(1.0f - cpfexp(-spring->damping*spring->dt/spring->nMass));
spring->target_vrn = vrn + v_damp;
apply_impulses(a, b, spring->r1, spring->r2, cpvmult(spring->n, v_damp*spring->nMass));
}
void
cpArbiterUpdate(cpArbiter *arb, cpContact *contacts, int numContacts, cpCollisionHandler *handler, cpShape *a, cpShape *b)
{
// Iterate over the possible pairs to look for hash value matches.
for(int i=0; i<numContacts; i++){
cpContact *con = &contacts[i];
for(int j=0; j<arb->numContacts; j++){
cpContact *old = &arb->contacts[j];
// This could trigger false positives, but is fairly unlikely nor serious if it does.
if(con->hash == old->hash){
// Copy the persistant contact information.
con->jnAcc = old->jnAcc;
con->jtAcc = old->jtAcc;
}
}
}
arb->contacts = contacts;
arb->numContacts = numContacts;
arb->handler = handler;
arb->swappedColl = (a->collision_type != handler->a);
arb->e = a->e * b->e;
arb->u = a->u * b->u;
// Currently all contacts will have the same normal.
// This may change in the future.
cpVect n = (numContacts ? contacts[0].n : cpvzero);
cpVect surface_vr = cpvsub(a->surface_v, b->surface_v);
arb->surface_vr = cpvsub(surface_vr, cpvmult(n, cpvdot(surface_vr, n)));
// For collisions between two similar primitive types, the order could have been swapped.
arb->a = a; arb->body_a = a->body;
arb->b = b; arb->body_b = b->body;
// mark it as new if it's been cached
if(arb->state == cpArbiterStateCached) arb->state = cpArbiterStateFirstColl;
}
static void
draw(void)
{
ChipmunkDemoDefaultDrawImpl();
cpVect start = QUERY_START;
cpVect end = ChipmunkDemoMouse;
ChipmunkDebugDrawSegment(start, end, RGBAColor(0,1,0,1));
ChipmunkDemoPrintString("Query: Dist(%f) Point%s, ", cpvdist(start, end), cpvstr(end));
cpSegmentQueryInfo segInfo = {};
if(cpSpaceSegmentQueryFirst(space, start, end, CP_ALL_LAYERS, CP_NO_GROUP, &segInfo)){
cpVect point = cpSegmentQueryHitPoint(start, end, segInfo);
// Draw red over the occluded part of the query
ChipmunkDebugDrawSegment(point, end, RGBAColor(1,0,0,1));
// Draw a little blue surface normal
ChipmunkDebugDrawSegment(point, cpvadd(point, cpvmult(segInfo.n, 16)), RGBAColor(0,0,1,1));
// Draw a little red dot on the hit point.
ChipmunkDebugDrawPoints(3, 1, &point, RGBAColor(1,0,0,1));
ChipmunkDemoPrintString("Segment Query: Dist(%f) Normal%s", cpSegmentQueryHitDist(start, end, segInfo), cpvstr(segInfo.n));
} else {
ChipmunkDemoPrintString("Segment Query (None)");
}
cpNearestPointQueryInfo nearestInfo = {};
cpSpaceNearestPointQueryNearest(space, ChipmunkDemoMouse, 100.0, CP_ALL_LAYERS, CP_NO_GROUP, &nearestInfo);
if(nearestInfo.shape){
// Draw a grey line to the closest shape.
ChipmunkDebugDrawPoints(3, 1, &ChipmunkDemoMouse, RGBAColor(0.5, 0.5, 0.5, 1.0));
ChipmunkDebugDrawSegment(ChipmunkDemoMouse, nearestInfo.p, RGBAColor(0.5, 0.5, 0.5, 1.0));
// Draw a red bounding box around the shape under the mouse.
if(nearestInfo.d < 0) ChipmunkDebugDrawBB(cpShapeGetBB(nearestInfo.shape), RGBAColor(1,0,0,1));
}
}
static void
update(int ticks)
{
int steps = 1;
cpFloat dt = 1.0f/60.0f/(cpFloat)steps;
if(!emitterInstance.blocked && emitterInstance.queue){
emitterInstance.queue--;
cpBody *body = cpSpaceAddBody(space, cpBodyNew(1.0f, cpMomentForCircle(1.0f, 15.0f, 0.0f, cpvzero)));
body->p = emitterInstance.position;
body->v = cpvmult(cpv(frand_unit(), frand_unit()), 100.0f);
cpShape *shape = cpSpaceAddShape(space, cpCircleShapeNew(body, 15.0f, cpvzero));
shape->collision_type = BALL_TYPE;
}
for(int i=0; i<steps; i++){
cpSpaceStep(space, dt);
}
}
