Exemplo n.º 1
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, 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;
		}
	}
}
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.º 4
0
static void
cpCircleShapeSegmentQuery(cpCircleShape *circle, cpVect a, cpVect b, cpFloat radius, cpSegmentQueryInfo *info)
{
	CircleSegmentQuery((cpShape *)circle, circle->tc, circle->r, a, b, radius, info);
}