C++ cpvcross函数代码示例

static void
cpPolyShapeSegmentQuery(cpPolyShape *poly, cpVect a, cpVect b, cpSegmentQueryInfo *info)
{
	cpSplittingPlane *axes = poly->tPlanes;
	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 = (cpShape *)poly;
			info->t = t;
			info->n = n;
		}
	}
}

static void
preStep(cpPulleyJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	cpBody* b1 = joint->constraint.a;
	cpBody* b2 = joint->constraint.b;
	
	joint->r1 = cpvrotate(joint->anchr1, b1->rot);
	joint->r2 = cpvrotate(joint->anchr2, b2->rot);
    
	cpVect p1 = cpvadd(b1->p, joint->r1);
	cpVect p2 = cpvadd(b2->p, joint->r2);
    
	//Catto claimed that these needed to be "grounded" pts
	cpVect s1 = cpBodyLocal2World(joint->c, joint->anchr3a);
	cpVect s2 = cpBodyLocal2World(joint->c, joint->anchr3b);
    
	// Get the pulley axes.
	joint->u1 = cpvsub(p1, s1);
	joint->u2 = cpvsub(p2, s2);
    
	// Lengths
	cpFloat length1 = cpvlength(joint->u1);
	cpFloat length2 = cpvlength(joint->u2);
    
	// Check constraints
	joint->u1 = (length1 > .01) ? cpvmult(joint->u1, 1.0f/length1) : cpvzero;
	joint->u2 = (length2 > .01) ? cpvmult(joint->u2, 1.0f/length2) : cpvzero;
    
	// Compute 'C'
	cpFloat C = joint->constant - length1 - joint->ratio * length2;
    
	// Set state based on lengths
	joint->state = (C > 0.0f) ? 0 : 1;
	joint->limitState1 = (length1 < joint->max1) ? 0 : 1;
	joint->limitState2 = (length2 < joint->max2) ? 0 : 1;
    
	// Compute effective mass.
	cpFloat cr1u1 = cpvcross(joint->r1, joint->u1);
	cpFloat cr2u2 = cpvcross(joint->r2, joint->u2);
    
	// Set Mass Limits
	joint->limitMass1 = b1->m_inv + b1->i_inv * cr1u1 * cr1u1;
	joint->limitMass2 = b2->m_inv + b2->i_inv * cr2u2 * cr2u2;
	joint->pulleyMass = joint->limitMass1 + joint->ratio * joint->ratio * joint->limitMass2;
	
	// Check against evil
	//cpAssert(joint->limitMass1 != 0.0f, "Calculated Pulley Limit(1) is Zero");
	//cpAssert(joint->limitMass2 != 0.0f, "Calculated Pulley Limit(2) is Zero");
	//cpAssert(joint->pulleyMass != 0.0f, "Calculated Pulley Mass is Zero");
	
	// We want the inverse's
	joint->limitMass1 = 1.0f / joint->limitMass1;
	joint->limitMass2 = 1.0f / joint->limitMass2;
	joint->pulleyMass = 1.0f / joint->pulleyMass;
    
	// Reset accumulations, could also warm start here
	joint->jnAcc = 0.0f;
	joint->jnAccLim1 = 0.0f;
	joint->jnAccLim2 = 0.0f;
}

static struct ClosestPoints
EPARecurse(const struct SupportContext *ctx, const int count, const struct MinkowskiPoint *hull, const int iteration)
{
	int mini = 0;
	cpFloat minDist = INFINITY;
	
	// TODO: precalculate this when building the hull and save a step.
	for(int j=0, i=count-1; j<count; i=j, j++){
		cpFloat d = ClosestDist(hull[i].ab, hull[j].ab);
		if(d < minDist){
			minDist = d;
			mini = i;
		}
	}
	
	struct MinkowskiPoint v0 = hull[mini];
	struct MinkowskiPoint v1 = hull[(mini + 1)%count];
	cpAssertSoft(!cpveql(v0.ab, v1.ab), "Internal Error: EPA vertexes are the same (%d and %d)", mini, (mini + 1)%count);
	
	struct MinkowskiPoint p = Support(ctx, cpvperp(cpvsub(v1.ab, v0.ab)));
	
#if DRAW_EPA
	cpVect verts[count];
	for(int i=0; i<count; i++) verts[i] = hull[i].ab;
	
	ChipmunkDebugDrawPolygon(count, verts, 0.0, RGBAColor(1, 1, 0, 1), RGBAColor(1, 1, 0, 0.25));
	ChipmunkDebugDrawSegment(v0.ab, v1.ab, RGBAColor(1, 0, 0, 1));
	
	ChipmunkDebugDrawDot(5, p.ab, LAColor(1, 1));
#endif
	
	cpFloat area2x = cpvcross(cpvsub(v1.ab, v0.ab), cpvadd(cpvsub(p.ab, v0.ab), cpvsub(p.ab, v1.ab)));
	if(area2x > 0.0f && iteration < MAX_EPA_ITERATIONS){
		int count2 = 1;
		struct MinkowskiPoint *hull2 = (struct MinkowskiPoint *)alloca((count + 1)*sizeof(struct MinkowskiPoint));
		hull2[0] = p;
		
		for(int i=0; i<count; i++){
			int index = (mini + 1 + i)%count;
			
			cpVect h0 = hull2[count2 - 1].ab;
			cpVect h1 = hull[index].ab;
			cpVect h2 = (i + 1 < count ? hull[(index + 1)%count] : p).ab;
			
			// TODO: Should this be changed to an area2x check?
			if(cpvcross(cpvsub(h2, h0), cpvsub(h1, h0)) > 0.0f){
				hull2[count2] = hull[index];
				count2++;
			}
		}
		
		return EPARecurse(ctx, count2, hull2, iteration + 1);
	} else {
		cpAssertWarn(iteration < WARN_EPA_ITERATIONS, "High EPA iterations: %d", iteration);
		return ClosestPointsNew(v0, v1);
	}
}

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

void
cpArbiterPreStep(cpArbiter *arb, cpFloat dt_inv)
{
	cpShape *shapea = arb->a;
	cpShape *shapeb = arb->b;
		
	arb->e = shapea->e * shapeb->e;
	arb->u = shapea->u * shapeb->u;
	arb->target_v = cpvsub(shapeb->surface_v, shapea->surface_v);

	cpBody *a = shapea->body;
	cpBody *b = shapeb->body;
	
	for(int i=0; i<arb->numContacts; i++){
		cpContact *con = &arb->contacts[i];
		
		// Calculate the offsets.
		con->r1 = cpvsub(con->p, a->p);
		con->r2 = cpvsub(con->p, b->p);
		
		// Calculate the mass normal.
		cpFloat mass_sum = a->m_inv + b->m_inv;
		
		cpFloat r1cn = cpvcross(con->r1, con->n);
		cpFloat r2cn = cpvcross(con->r2, con->n);
		cpFloat kn = mass_sum + a->i_inv*r1cn*r1cn + b->i_inv*r2cn*r2cn;
		con->nMass = 1.0f/kn;
		
		// Calculate the mass tangent.
		cpVect t = cpvperp(con->n);
		cpFloat r1ct = cpvcross(con->r1, t);
		cpFloat r2ct = cpvcross(con->r2, t);
		cpFloat kt = mass_sum + a->i_inv*r1ct*r1ct + b->i_inv*r2ct*r2ct;
		con->tMass = 1.0f/kt;
				
		// Calculate the target bias velocity.
		con->bias = -cp_bias_coef*dt_inv*cpfmin(0.0f, con->dist + cp_collision_slop);
		con->jBias = 0.0f;
		
		// Calculate the target bounce velocity.
		cpVect v1 = cpvadd(a->v, cpvmult(cpvperp(con->r1), a->w));
		cpVect v2 = cpvadd(b->v, cpvmult(cpvperp(con->r2), b->w));
		con->bounce = cpvdot(con->n, cpvsub(v2, v1))*arb->e;
		
		// Apply the previous accumulated impulse.
		cpVect j = cpvadd(cpvmult(con->n, con->jnAcc), cpvmult(t, con->jtAcc));
		cpBodyApplyImpulse(a, cpvneg(j), con->r1);
		cpBodyApplyImpulse(b, j, con->r2);
	}
}

// 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 int
QHullPartition(cpVect *verts, int count, cpVect a, cpVect b, cpFloat tol)
{
	if(count == 0) return 0;
	
	cpFloat max = 0;
	int pivot = 0;
	
	cpVect delta = cpvsub(b, a);
	cpFloat valueTol = tol*cpvlength(delta);
	
	int head = 0;
	for(int tail = count-1; head <= tail;){
		cpFloat value = cpvcross(delta, cpvsub(verts[head], a));
		if(value > valueTol){
			if(value > max){
				max = value;
				pivot = head;
			}
			
			head++;
		} else {
			SWAP(verts[head], verts[tail]);
			tail--;
		}
	}
	
	// move the new pivot to the front if it's not already there.
	if(pivot != 0) SWAP(verts[0], verts[pivot]);
	return head;
}

void
cpBodyApplyForce(cpBody *body, cpVect force, cpVect r)
{
	cpBodyActivate(body);
	body->f = cpvadd(body->f, force);
	body->t += cpvcross(r, force);
}

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 void
preStep(cpGrooveJoint *joint, cpFloat dt, cpFloat dt_inv)
{
	cpBody *a = joint->constraint.a;
	cpBody *b = joint->constraint.b;
	
	// calculate endpoints in worldspace
	cpVect ta = cpBodyLocal2World(a, joint->grv_a);
	cpVect tb = cpBodyLocal2World(a, joint->grv_b);

	// calculate axis
	cpVect n = cpvrotate(joint->grv_n, a->rot);
	cpFloat d = cpvdot(ta, n);
	
	joint->grv_tn = n;
	joint->r2 = cpvrotate(joint->anchr2, b->rot);
	
	// calculate tangential distance along the axis of r2
	cpFloat td = cpvcross(cpvadd(b->p, joint->r2), n);
	// calculate clamping factor and r2
	if(td <= cpvcross(ta, n)){
		joint->clamp = 1.0f;
		joint->r1 = cpvsub(ta, a->p);
	} else if(td >= cpvcross(tb, n)){
		joint->clamp = -1.0f;
		joint->r1 = cpvsub(tb, a->p);
	} else {
		joint->clamp = 0.0f;
		joint->r1 = cpvsub(cpvadd(cpvmult(cpvperp(n), -td), cpvmult(n, d)), a->p);
	}
	
	// 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, -joint->constraint.biasCoef*dt_inv), joint->constraint.maxBias);
	
	// apply accumulated impulse
	apply_impulses(a, b, joint->r1, joint->r2, joint->jAcc);
}

cpFloat
cpAreaForPoly(const int numVerts, const cpVect *verts)
{
	cpFloat area = 0.0f;
	for(int i=0; i<numVerts; i++){
		area += cpvcross(verts[i], verts[(i+1)%numVerts]);
	}
	
	return area/2.0f;
}

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

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