Exemplo n.º 1
0
static void
cpPolyShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	cpPolyShape *poly = (cpPolyShape *)shape;
	cpPolyShapeAxis *axes = poly->tAxes;
	cpVect *verts = poly->tVerts;
	int numVerts = poly->numVerts;
	
	for(int i=0; i<numVerts; i++){
		cpVect n = axes[i].n;
		cpFloat an = cpvdot(a, n);
		if(axes[i].d > an) continue;
		
		cpFloat bn = cpvdot(b, n);
		cpFloat t = (axes[i].d - an)/(bn - an);
		if(t < 0.0f || 1.0f < t) continue;
		
		cpVect point = cpvlerp(a, b, t);
		cpFloat dt = -cpvcross(n, point);
		cpFloat dtMin = -cpvcross(n, verts[i]);
		cpFloat dtMax = -cpvcross(n, verts[(i+1)%numVerts]);
		
		if(dtMin <= dt && dt <= dtMax){
			info->shape = shape;
			info->t = t;
			info->n = n;
		}
	}
}
Exemplo n.º 2
0
static cpFloat
FindSteiner(int count, cpVect *verts, struct Notch notch)
{
	cpFloat min = INFINITY;
	cpFloat feature = -1.0;
	
	for(int i=1; i<count-1; i++){
		int index = (notch.i + i)%count;
		
		cpVect seg_a = verts[index];
		cpVect seg_b = verts[Next(index, count)];
		
		cpFloat thing_a = cpvcross(notch.n, cpvsub(seg_a, notch.v));
		cpFloat thing_b = cpvcross(notch.n, cpvsub(seg_b, notch.v));
		if(thing_a*thing_b <= 0.0){
			cpFloat t = thing_a/(thing_a - thing_b);
			cpFloat dist = cpvdot(notch.n, cpvsub(cpvlerp(seg_a, seg_b, t), notch.v));
			
			if(dist >= 0.0 && dist <= min){
				min = dist;
				feature = index + t;
			}
		}
	}
	
	return feature;
}
Exemplo n.º 3
0
static void
display(void)
{
	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++;
		step = cpFalse;
	}
  
	if(drawBBs) cpSpaceEachShape(space, drawShapeBB, NULL);
	
	drawInstructions();
	drawInfo();
	drawString(-300, -200, ChipmunkDemoMessageString);
		
	glutSwapBuffers();
	glClear(GL_COLOR_BUFFER_BIT);
}
Exemplo n.º 4
0
cpFloat
cpMomentForSegment(cpFloat m, cpVect a, cpVect b, cpFloat r)
{
	cpVect offset = cpvlerp(a, b, 0.5f);
	
	// This approximates the shape as a box for rounded segments, but it's quite close.
	cpFloat length = cpvdist(b, a) + 2.0f*r;
	return m*((length*length + 4.0f*r*r)/12.0f + cpvlengthsq(offset));
}
Exemplo n.º 5
0
static struct cpShapeMassInfo
cpSegmentShapeMassInfo(cpFloat mass, cpVect a, cpVect b, cpFloat r)
{
	struct cpShapeMassInfo info = {
		mass, cpMomentForBox(1.0f, cpvdist(a, b) + 2.0f*r, 2.0f*r), // TODO is an approximation.
		cpvlerp(a, b, 0.5f),
		cpAreaForSegment(a, b, r),
	};
	
	return info;
}
static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
{
	struct cpSplittingPlane *planes = poly->planes;
	int count = poly->count;
	cpFloat r = poly->r;
	cpFloat rsum = r + r2;
	
	for(int i=0; i<count; i++){
		cpVect n = planes[i].n;
		cpFloat an = cpvdot(a, n);
		cpFloat d =  an - cpvdot(planes[i].v0, n) - rsum;
		if(d < 0.0f) continue;
		
		cpFloat bn = cpvdot(b, n);
		cpFloat t = d/(an - bn);
		if(t < 0.0f || 1.0f < t) continue;
		
		cpVect point = cpvlerp(a, b, t);
		cpFloat dt = cpvcross(n, point);
		cpFloat dtMin = cpvcross(n, planes[(i - 1 + count)%count].v0);
		cpFloat dtMax = cpvcross(n, planes[i].v0);
		
		if(dtMin <= dt && dt <= dtMax){
			info->shape = (cpShape *)poly;
			info->point = cpvsub(cpvlerp(a, b, t), cpvmult(n, r2));
			info->normal = n;
			info->alpha = t;
		}
	}
	
	// Also check against the beveled vertexes.
	if(rsum > 0.0f){
		for(int i=0; i<count; i++){
			cpSegmentQueryInfo circle_info = {NULL, b, cpvzero, 1.0f};
			CircleSegmentQuery(&poly->shape, planes[i].v0, r, a, b, r2, &circle_info);
			if(circle_info.alpha < info->alpha) (*info) = circle_info;
		}
	}
}
Exemplo n.º 7
0
// 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);
                }
            }
        }
    }
}
Exemplo n.º 8
0
static void
cpSegmentShapeSegmentQuery(cpShape *shape, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
    cpSegmentShape *seg = (cpSegmentShape *)shape;
    cpVect n = seg->tn;
    // flip n if a is behind the axis
    if(cpvdot(a, n) < cpvdot(seg->ta, n))
        n = cpvneg(n);

    cpFloat an = cpvdot(a, n);
    cpFloat bn = cpvdot(b, n);
    cpFloat d = cpvdot(seg->ta, n) + seg->r;

    cpFloat t = (d - an)/(bn - an);
    if(0.0f < t && t < 1.0f) {
        cpVect point = cpvlerp(a, b, t);
        cpFloat dt = -cpvcross(seg->tn, point);
        cpFloat dtMin = -cpvcross(seg->tn, seg->ta);
        cpFloat dtMax = -cpvcross(seg->tn, seg->tb);

        if(dtMin < dt && dt < dtMax) {
            info->shape = shape;
            info->t = t;
            info->n = n;

            return; // don't continue on and check endcaps
        }
    }

    if(seg->r) {
        cpSegmentQueryInfo info1;
        info1.shape = NULL;
        cpSegmentQueryInfo info2;
        info2.shape = NULL;
        circleSegmentQuery(shape, seg->ta, seg->r, a, b, &info1);
        circleSegmentQuery(shape, seg->tb, seg->r, a, b, &info2);

        if(info1.shape && !info2.shape) {
            (*info) = info1;
        } else if(info2.shape && !info1.shape) {
            (*info) = info2;
        } else if(info1.shape && info2.shape) {
            if(info1.t < info2.t) {
                (*info) = info1;
            } else {
                (*info) = info2;
            }
        }
    }
}
Exemplo n.º 9
0
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);
				}
			}
		}
	}
}
Exemplo n.º 10
0
void Slice::ClipPoly(cpSpace *space, cpShape *shape, cpVect n, cpFloat dist)
{
	cpBody *body = cpShapeGetBody(shape);
	
	int count = cpPolyShapeGetCount(shape);
	int clippedCount = 0;
	
	cpVect *clipped = (cpVect *)alloca((count + 1)*sizeof(cpVect));
	
	for(int i=0, j=count-1; i<count; j=i, i++){
		cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(shape, j));
		cpFloat a_dist = cpvdot(a, n) - dist;
		
		if(a_dist < 0.0){
			clipped[clippedCount] = a;
			clippedCount++;
		}
		
		cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(shape, i));
		cpFloat b_dist = cpvdot(b, n) - dist;
		
		if(a_dist*b_dist < 0.0f){
			cpFloat t = cpfabs(a_dist)/(cpfabs(a_dist) + cpfabs(b_dist));
			
			clipped[clippedCount] = cpvlerp(a, b, t);
			clippedCount++;
		}
	}
	
	cpVect centroid = cpCentroidForPoly(clippedCount, clipped);
	cpFloat mass = cpAreaForPoly(clippedCount, clipped, 0.0f)*DENSITY;
	cpFloat moment = cpMomentForPoly(mass, clippedCount, clipped, cpvneg(centroid), 0.0f);
	
	cpBody *new_body = cpSpaceAddBody(space, cpBodyNew(mass, moment));
	cpBodySetPosition(new_body, centroid);
	cpBodySetVelocity(new_body, cpBodyGetVelocityAtWorldPoint(body, centroid));
	cpBodySetAngularVelocity(new_body, cpBodyGetAngularVelocity(body));
	
	cpTransform transform = cpTransformTranslate(cpvneg(centroid));
	cpShape *new_shape = cpSpaceAddShape(space, cpPolyShapeNew(new_body, clippedCount, clipped, transform, 0.0));
	// Copy whatever properties you have set on the original shape that are important
	cpShapeSetFriction(new_shape, cpShapeGetFriction(shape));
}
Exemplo n.º 11
0
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);
}
Exemplo n.º 12
0
static void
ApproximateConcaveDecomposition(cpVect *verts, int count, cpFloat tol, cpPolylineSet *set)
{
	int first;
	cpVect *hullVerts = alloca(count*sizeof(cpVect));
	int hullCount = cpConvexHull(count, verts, hullVerts, &first, 0.0);
	
	if(hullCount != count){
		struct Notch notch = DeepestNotch(count, verts, hullCount, hullVerts, first, tol);
		
		if(notch.d > tol){
			cpFloat steiner_it = FindSteiner(count, verts, notch);
			
			if(steiner_it >= 0.0){
				int steiner_i = (int)steiner_it;
				cpVect steiner = cpvlerp(verts[steiner_i], verts[Next(steiner_i, count)], steiner_it - steiner_i);
				
				// Vertex counts NOT including the steiner point.
				int sub1_count = (steiner_i - notch.i + count)%count + 1;
				int sub2_count = count - (steiner_i - notch.i + count)%count;
				cpVect *scratch = alloca((IMAX(sub1_count, sub2_count) + 1)*sizeof(cpVect));
				
				for(int i=0; i<sub1_count; i++) scratch[i] = verts[(notch.i + i)%count];
				scratch[sub1_count] = steiner;
				ApproximateConcaveDecomposition(scratch, sub1_count + 1, tol, set);
				
				for(int i=0; i<sub2_count; i++) scratch[i] = verts[(steiner_i + 1 + i)%count];
				scratch[sub2_count] = steiner;
				ApproximateConcaveDecomposition(scratch, sub2_count + 1, tol, set);
				
				return;
			}
		}
	}
	
	cpPolyline *hull = cpPolylineMake(hullCount + 1);
	
	memcpy(hull->verts, hullVerts, hullCount*sizeof(cpVect));
	hull->verts[hullCount] = hullVerts[0];
	hull->count = hullCount + 1;
	
	cpPolylineSetPush(set, hull);
}
Exemplo n.º 13
0
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);
}
Exemplo n.º 14
0
static void
cpSegmentShapeSegmentQuery(cpSegmentShape *seg, cpVect a, cpVect b, cpFloat r2, cpSegmentQueryInfo *info)
{
	cpVect n = seg->tn;
	cpFloat d = cpvdot(cpvsub(seg->ta, a), n);
	cpFloat r = seg->r + r2;
	
	cpVect flipped_n = (d > 0.0f ? cpvneg(n) : n);
	cpVect seg_offset = cpvsub(cpvmult(flipped_n, r), a);
	
	// Make the endpoints relative to 'a' and move them by the thickness of the segment.
	cpVect seg_a = cpvadd(seg->ta, seg_offset);
	cpVect seg_b = cpvadd(seg->tb, seg_offset);
	cpVect delta = cpvsub(b, a);
	
	if(cpvcross(delta, seg_a)*cpvcross(delta, seg_b) <= 0.0f){
		cpFloat d_offset = d + (d > 0.0f ? -r : r);
		cpFloat ad = -d_offset;
		cpFloat bd = cpvdot(delta, n) - d_offset;
		
		if(ad*bd < 0.0f){
			cpFloat t = ad/(ad - bd);
			
			info->shape = (cpShape *)seg;
			info->point = cpvsub(cpvlerp(a, b, t), cpvmult(flipped_n, r2));
			info->normal = flipped_n;
			info->alpha = t;
		}
	} else if(r != 0.0f){
		cpSegmentQueryInfo info1 = {NULL, b, cpvzero, 1.0f};
		cpSegmentQueryInfo info2 = {NULL, b, cpvzero, 1.0f};
		CircleSegmentQuery((cpShape *)seg, seg->ta, seg->r, a, b, r2, &info1);
		CircleSegmentQuery((cpShape *)seg, seg->tb, seg->r, a, b, r2, &info2);
		
		if(info1.alpha < info2.alpha){
			(*info) = info1;
		} else {
			(*info) = info2;
		}
	}
}
static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	cpSplittingPlane *axes = poly->tPlanes;
	cpVect *verts = poly->tVerts;
	int numVerts = poly->numVerts;
	cpFloat r = poly->r;
	
	for(int i=0; i<numVerts; i++){
		cpVect n = axes[i].n;
		cpFloat an = cpvdot(a, n);
		cpFloat d = axes[i].d + r - an;
		if(d > 0.0f) continue;
		
		cpFloat bn = cpvdot(b, n);
		cpFloat t = d/(bn - an);
		if(t < 0.0f || 1.0f < t) continue;
		
		cpVect point = cpvlerp(a, b, t);
		cpFloat dt = -cpvcross(n, point);
		cpFloat dtMin = -cpvcross(n, verts[(i - 1 + numVerts)%numVerts]);
		cpFloat dtMax = -cpvcross(n, verts[i]);
		
		if(dtMin <= dt && dt <= dtMax){
			info->shape = (cpShape *)poly;
			info->t = t;
			info->n = n;
		}
	}
	
	// Also check against the beveled vertexes.
	if(r > 0.0f){
		for(int i=0; i<numVerts; i++){
			cpSegmentQueryInfo circle_info = {NULL, 1.0f, cpvzero};
			CircleSegmentQuery(&poly->shape, verts[i], r, a, b, &circle_info);
			if(circle_info.t < info->t) (*info) = circle_info;
		}
	}
}
Exemplo n.º 16
0
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));
		}
	}
}
Exemplo n.º 17
0
/* Mouse handling is a bit tricky. We want the user to move
 * tiles using the mouse but because tiles are dynamic bodies
 * managed by Chipmunk2D, we cannot directly control them.
 * This is resolved by creating a pivot joint between an
 * invisible mouse body that we can control and the tile body
 * that we cannot directly control.
 */
static void apply_mouse_motion(struct state* state)
{
    struct mouse m;
    update_mouse(&m);
    int w, h;
    get_screen_size(&w, &h);
    int x = m.x_position * w;
    int y = m.y_position * h;
    cpVect mouse_pos = cpv(x, y);
    cpVect new_point =
        cpvlerp(cpBodyGetPosition(state->mouse_body), mouse_pos, 0.25f);
    cpBodySetVelocity(
        state->mouse_body,
        cpvmult(cpvsub(new_point, cpBodyGetPosition(state->mouse_body)),
                60.0f));
    cpBodySetPosition(state->mouse_body, new_point);
    if (m.left_click && state->mouse_joint == NULL) {
        cpFloat radius = 5.0;
        cpPointQueryInfo info = { 0 };
        cpShape* shape = cpSpacePointQueryNearest(state->space, mouse_pos,
                                                  radius, GRAB_FILTER, &info);
        if (shape && cpBodyGetMass(cpShapeGetBody(shape)) < INFINITY) {
            cpVect nearest = (info.distance > 0.0f ? info.point : mouse_pos);
            cpBody* body = cpShapeGetBody(shape);
            state->mouse_joint =
                cpPivotJointNew2(state->mouse_body, body, cpvzero,
                                 cpBodyWorldToLocal(body, nearest));
            cpConstraintSetMaxForce(state->mouse_joint, 5000000.0f);
            cpConstraintSetErrorBias(state->mouse_joint,
                                     cpfpow(1.0f - 0.15f, 60.0f));
            cpSpaceAddConstraint(state->space, state->mouse_joint);
        }
    }
    if (m.left_click == false && state->mouse_joint != NULL) {
        cpSpaceRemoveConstraint(state->space, state->mouse_joint);                                                 
        cpConstraintFree(state->mouse_joint);                                                               
        state->mouse_joint = NULL;  
    }
}
Exemplo n.º 18
0
cpBool Buoyancy::WaterPreSolve(cpArbiter *arb, cpSpace *space, void *ptr)
{
    CP_ARBITER_GET_SHAPES(arb, water, poly);
    cpBody *body = cpShapeGetBody(poly);

    // Get the top of the water sensor bounding box to use as the water level.
    cpFloat level = cpShapeGetBB(water).t;

    // Clip the polygon against the water level
    int count = cpPolyShapeGetCount(poly);
    int clippedCount = 0;
#ifdef _MSC_VER
    // MSVC is pretty much the only compiler in existence that doesn't support variable sized arrays.
    cpVect clipped[10];
#else
    cpVect clipped[count + 1];
#endif

    for(int i=0, j=count-1; i<count; j=i, i++){
        cpVect a = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, j));
        cpVect b = cpBodyLocalToWorld(body, cpPolyShapeGetVert(poly, i));

        if(a.y < level){
            clipped[clippedCount] = a;
            clippedCount++;
        }

        cpFloat a_level = a.y - level;
        cpFloat b_level = b.y - level;

        if(a_level*b_level < 0.0f){
            cpFloat t = cpfabs(a_level)/(cpfabs(a_level) + cpfabs(b_level));

            clipped[clippedCount] = cpvlerp(a, b, t);
            clippedCount++;
        }
    }

    // Calculate buoyancy from the clipped polygon area
    cpFloat clippedArea = cpAreaForPoly(clippedCount, clipped, 0.0f);
    cpFloat displacedMass = clippedArea*FLUID_DENSITY;
    cpVect centroid = cpCentroidForPoly(clippedCount, clipped);

    cpDataPointer data = ptr;
    DrawPolygon(clippedCount, clipped, 0.0f, RGBAColor(0, 0, 1, 1), RGBAColor(0, 0, 1, 0.1f), data);
    DrawDot(5, centroid, RGBAColor(0, 0, 1, 1), data);

    cpFloat dt = cpSpaceGetCurrentTimeStep(space);
    cpVect g = cpSpaceGetGravity(space);

    // Apply the buoyancy force as an impulse.
    cpBodyApplyImpulseAtWorldPoint(body, cpvmult(g, -displacedMass*dt), centroid);

    // Apply linear damping for the fluid drag.
    cpVect v_centroid = cpBodyGetVelocityAtWorldPoint(body, centroid);
    cpFloat k = k_scalar_body(body, centroid, cpvnormalize(v_centroid));
    cpFloat damping = clippedArea*FLUID_DRAG*FLUID_DENSITY;
    cpFloat v_coef = cpfexp(-damping*dt*k); // linear drag
    //	cpFloat v_coef = 1.0/(1.0 + damping*dt*cpvlength(v_centroid)*k); // quadratic drag
    cpBodyApplyImpulseAtWorldPoint(body, cpvmult(cpvsub(cpvmult(v_centroid, v_coef), v_centroid), 1.0/k), centroid);

    // Apply angular damping for the fluid drag.
    cpVect cog = cpBodyLocalToWorld(body, cpBodyGetCenterOfGravity(body));
    cpFloat w_damping = cpMomentForPoly(FLUID_DRAG*FLUID_DENSITY*clippedArea, clippedCount, clipped, cpvneg(cog), 0.0f);
    cpBodySetAngularVelocity(body, cpBodyGetAngularVelocity(body)*cpfexp(-w_damping*dt/cpBodyGetMoment(body)));

    return cpTrue;
}