示例#1
0
// This one is complicated and gross. Just don't go there...
// TODO: Comment me!
static int
seg2poly(const cpShape *shape1, const cpShape *shape2, cpContact *arr)
{
	cpSegmentShape *seg = (cpSegmentShape *)shape1;
	cpPolyShape *poly = (cpPolyShape *)shape2;
	cpSplittingPlane *planes = poly->tPlanes;
	
	cpFloat segD = cpvdot(seg->tn, seg->ta);
	cpFloat minNorm = cpPolyShapeValueOnAxis(poly, seg->tn, segD) - seg->r;
	cpFloat minNeg = cpPolyShapeValueOnAxis(poly, cpvneg(seg->tn), -segD) - seg->r;
	if(minNeg > 0.0f || minNorm > 0.0f) return 0;
	
	int mini = 0;
	cpFloat poly_min = segValueOnAxis(seg, planes->n, planes->d);
	if(poly_min > 0.0f) return 0;
	for(int i=0; i<poly->numVerts; i++){
		cpFloat dist = segValueOnAxis(seg, planes[i].n, planes[i].d);
		if(dist > 0.0f){
			return 0;
		} else if(dist > poly_min){
			poly_min = dist;
			mini = i;
		}
	}
	
	int num = 0;
	
	cpVect poly_n = cpvneg(planes[mini].n);
	
	cpVect va = cpvadd(seg->ta, cpvmult(poly_n, seg->r));
	cpVect vb = cpvadd(seg->tb, cpvmult(poly_n, seg->r));
	if(cpPolyShapeContainsVert(poly, va))
		cpContactInit(nextContactPoint(arr, &num), va, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 0));
	if(cpPolyShapeContainsVert(poly, vb))
		cpContactInit(nextContactPoint(arr, &num), vb, poly_n, poly_min, CP_HASH_PAIR(seg->shape.hashid, 1));
	
	// Floating point precision problems here.
	// This will have to do for now.
//	poly_min -= cp_collision_slop; // TODO is this needed anymore?
	
	if(minNorm >= poly_min || minNeg >= poly_min) {
		if(minNorm > minNeg)
			findPointsBehindSeg(arr, &num, seg, poly, minNorm, 1.0f);
		else
			findPointsBehindSeg(arr, &num, seg, poly, minNeg, -1.0f);
	}
	
	// If no other collision points are found, try colliding endpoints.
	if(num == 0){
		cpVect poly_a = poly->tVerts[mini];
		cpVect poly_b = poly->tVerts[(mini + 1)%poly->numVerts];
		
		if(circle2circleQuery(seg->ta, poly_a, seg->r, 0.0f, arr)) return 1;
		if(circle2circleQuery(seg->tb, poly_a, seg->r, 0.0f, arr)) return 1;
		if(circle2circleQuery(seg->ta, poly_b, seg->r, 0.0f, arr)) return 1;
		if(circle2circleQuery(seg->tb, poly_b, seg->r, 0.0f, arr)) return 1;
	}

	return num;
}
// Submitted by LegoCyclon
static int
seg2seg(const cpShape* shape1, const cpShape* shape2, cpContact* con)
{
	cpSegmentShape* seg1 = (cpSegmentShape *)shape1;
	cpSegmentShape* seg2 = (cpSegmentShape *)shape2;
	
	cpVect v1 = cpvsub(seg1->tb, seg1->ta);
	cpVect v2 = cpvsub(seg2->tb, seg2->ta);
	cpFloat v1lsq = cpvlengthsq(v1);
	cpFloat v2lsq = cpvlengthsq(v2);
	// project seg2 onto seg1
	cpVect p1a = cpvproject(cpvsub(seg2->ta, seg1->ta), v1);
	cpVect p1b = cpvproject(cpvsub(seg2->tb, seg1->ta), v1);
	// project seg1 onto seg2
	cpVect p2a = cpvproject(cpvsub(seg1->ta, seg2->ta), v2);
	cpVect p2b = cpvproject(cpvsub(seg1->tb, seg2->ta), v2);
	
	// clamp projections to segment endcaps
	if (cpvdot(p1a, v1) < 0.0f)
	 p1a = cpvzero;
	else if (cpvdot(p1a, v1) > 0.0f && cpvlengthsq(p1a) > v1lsq)
	 p1a = v1;
	if (cpvdot(p1b, v1) < 0.0f)
	 p1b = cpvzero;
	else if (cpvdot(p1b, v1) > 0.0f && cpvlengthsq(p1b) > v1lsq)
	 p1b = v1;
	if (cpvdot(p2a, v2) < 0.0f)
	 p2a = cpvzero;
	else if (cpvdot(p2a, v2) > 0.0f && cpvlengthsq(p2a) > v2lsq)
	 p2a = v2;
	if (cpvdot(p2b, v2) < 0.0f)
	 p2b = cpvzero;
	else if (cpvdot(p2b, v2) > 0.0f && cpvlengthsq(p2b) > v2lsq)
	 p2b = v2;
	
	p1a = cpvadd(p1a, seg1->ta);
	p1b = cpvadd(p1b, seg1->ta);
	p2a = cpvadd(p2a, seg2->ta);
	p2b = cpvadd(p2b, seg2->ta);
	
	int num = 0;
	
	if (!circle2circleQuery(p1a, p2a, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	if (!circle2circleQuery(p1b, p2b, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	if (!circle2circleQuery(p1a, p2b, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	if (!circle2circleQuery(p1b, p2a, seg1->r, seg2->r, nextContactPoint(con, &num)))
	 --num;
	
	return num;
}
示例#3
0
// 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;
}
示例#4
0
static int
segmentEncapQuery(cpVect p1, cpVect p2, cpFloat r1, cpFloat r2, cpContact *con, cpVect tangent)
{
	int count = circle2circleQuery(p1, p2, r1, r2, con);
//	printf("dot %5.2f\n", cpvdot(con[0].n, tangent));
	return (cpvdot(con[0].n, tangent) >= 0.0 ? count : 0);
}
示例#5
0
static int
circle2segment(const cpCircleShape *circleShape, const cpSegmentShape *segmentShape, cpContact *con)
{
	cpVect seg_a = segmentShape->ta;
	cpVect seg_b = segmentShape->tb;
	cpVect center = circleShape->tc;
	
	cpVect seg_delta = cpvsub(seg_b, seg_a);
	cpFloat closest_t = cpfclamp01(cpvdot(seg_delta, cpvsub(center, seg_a))/cpvlengthsq(seg_delta));
	cpVect closest = cpvadd(seg_a, cpvmult(seg_delta, closest_t));
	
	if(circle2circleQuery(center, closest, circleShape->r, segmentShape->r, con)){
		cpVect n = con[0].n;
		
		// Reject endcap collisions if tangents are provided.
		if(
			(closest_t == 0.0f && cpvdot(n, segmentShape->a_tangent) < 0.0) ||
			(closest_t == 1.0f && cpvdot(n, segmentShape->b_tangent) < 0.0)
		) return 0;
		
		return 1;
	} else {
		return 0;
	}
}
示例#6
0
// Collide circle shapes.
static int
circle2circle(cpShape *shape1, cpShape *shape2, cpContact **arr)
{
	cpCircleShape *circ1 = (cpCircleShape *)shape1;
	cpCircleShape *circ2 = (cpCircleShape *)shape2;
	
	return circle2circleQuery(circ1->tc, circ2->tc, circ1->r, circ2->r, arr);
}
示例#7
0
// 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);
	}
}
示例#8
0
// 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);
	}
}