C++ cpAssert函数代码示例

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 > cp_collision_slop) ? cpvmult(joint->u1, 1.0f/length1) : cpvzero;
	joint->u2 = (length2 > cp_collision_slop) ? 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;
}

void
cpBodySleepWithGroup(cpBody *body, cpBody *group){
	cpAssert(!cpBodyIsStatic(body) && !cpBodyIsRogue(body), "Rogue and static bodies cannot be put to sleep.");

	cpSpace *space = body->space;
	cpAssert(space, "Cannot put a body to sleep that has not been added to a space.");
	cpAssert(!space->locked, "Bodies can not be put to sleep during a query or a call to cpSpaceSte(). Put these calls into a post-step callback.");
	cpAssert(!group || cpBodyIsSleeping(group), "Cannot use a non-sleeping body as a group identifier.");

	if(cpBodyIsSleeping(body)) return;

	for(cpShape *shape = body->shapesList; shape; shape = shape->next){
		cpShapeCacheBB(shape);
		cpSpaceHashRemove(space->activeShapes, shape, shape->hashid);
		cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
	}

	if(group){
		cpBody *root = componentNodeRoot(group);

		cpComponentNode node = {root, root->node.next, 0, 0.0f};
		body->node = node;
		root->node.next = body;
	} else {
		cpComponentNode node = {NULL, body, 0, 0.0f};
		body->node = node;

		cpArrayPush(space->sleepingComponents, body);
	}

	cpArrayDeleteObj(space->bodies, body);
}

cpVect
cpPolyShapeGetVert(cpShape *shape, int idx)
{
	cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
	cpAssert(0 <= idx && idx < cpPolyShapeGetNumVerts(shape), "Index out of range.");

	return ((cpPolyShape *)shape)->verts[idx];
}

cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
	cpAssert(shape->body, "Cannot add a shape with a NULL body.");
	cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape), "Cannot add the same shape more than once.");
	cpAssertSpaceUnlocked(space);
	
	cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
	return shape;
}

void
cpPolyShapeSetVerts(cpShape *shape, int numVerts, cpVect *verts, cpVect offset)
{
	cpAssert(shape->klass == &polyClass, "Shape is not a poly shape.");
	cpPolyShapeDestroy(shape);
	setUpVerts((cpPolyShape *)shape, numVerts, verts, offset);
}

cpPulleyJoint *
cpPulleyJointInit(cpPulleyJoint *joint,
				  cpBody* a, cpBody* b, cpBody* c,
				  cpVect anchor1, cpVect anchor2,
				  cpVect anchor3a, cpVect anchor3b,
				  cpFloat ratio)
{
	cpConstraintInit((cpConstraint *)joint, &klass, a, b);
	
	joint->c = c;
	joint->anchr3a = anchor3a;
	joint->anchr3b = anchor3b;
	joint->anchr1 = anchor1;
	joint->anchr2 = anchor2;
	cpVect d1 = cpvsub(cpBodyLocal2World(a, anchor1), cpBodyLocal2World(c, anchor3a));
	cpVect d2 = cpvsub(cpBodyLocal2World(b, anchor2), cpBodyLocal2World(c, anchor3b));
	joint->dist1 = cpvlength(d1);
	joint->dist2 = cpvlength(d2);
	joint->ratio = ratio;
	
	cpAssert(ratio != 0.0f, "Pulley Ratio is Zero");
	
	// Calculate max and constant
	joint->constant = joint->dist1 + ratio * joint->dist2;
	joint->max1 = joint->constant - ratio * cp_min_pulley_len;
	joint->max2 = (joint->constant - cp_min_pulley_len) / joint->ratio;
	
	// Initialize
	joint->jnAcc = 0.0f;
	joint->jnAccLim1 = 0.0f;
	joint->jnAccLim2 = 0.0f;
	
	return joint;
}

void
cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
{
	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
	cpSegmentShape *seg = (cpSegmentShape *)shape;
	
	seg->r = radius;
}

void
cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
{
	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
	cpCircleShape *circle = (cpCircleShape *)shape;
	
	circle->c = offset;
}

void
cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
{
	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
	cpCircleShape *circle = (cpCircleShape *)shape;
	
	circle->r = radius;
}

void
cpSpaceRemoveConstraint(cpSpace *space, cpConstraint *constraint)
{
	cpAssert(cpArrayContains(space->constraints, constraint), "Cannot remove a constraint that was never added to the space.");
//	cpAssertSpaceUnlocked(space); Should be safe as long as its not from a constraint callback.
	
	cpArrayDeleteObj(space->constraints, constraint);
}

void
cpSpaceRemoveBody(cpSpace *space, cpBody *body)
{
	cpAssert(cpArrayContains(space->bodies, body), "Cannot remove a body that was never added to the space.");
	cpAssertSpaceUnlocked(space);
	
	cpArrayDeleteObj(space->bodies, body);
}

int
cpCollideShapes(const cpShape *a, const cpShape *b, cpContact *arr)
{
	// Their shape types must be in order.
	cpAssert(a->klass->type <= b->klass->type, "Collision shapes passed to cpCollideShapes() are not sorted.");
	
	collisionFunc cfunc = colfuncs[a->klass->type + b->klass->type*CP_NUM_SHAPES];
	return (cfunc) ? cfunc(a, b, arr) : 0;
}

cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
	cpAssert(!cpArrayContains(space->bodies, body), "Cannot add the same body more than once.");
//	cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
	
	cpArrayPush(space->bodies, body);
	
	return body;
}

void
cpSegmentShapeSetEndpoints(cpShape *shape, cpVect a, cpVect b)
{
	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
	cpSegmentShape *seg = (cpSegmentShape *)shape;
	
	seg->a = a;
	seg->b = b;
	seg->n = cpvperp(cpvnormalize(cpvsub(b, a)));
}

void
cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape)
{
	cpAssert(cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape), "Cannot remove a static shape that was never added to the space.");
	cpAssertSpaceUnlocked(space);
	
	removalContext context = {space, shape};
	cpHashSetFilter(space->contactSet, (cpHashSetFilterFunc)contactSetFilterRemovedShape, &context);
	cpSpaceHashRemove(space->staticShapes, shape, shape->hashid);
}