Example #1
0
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);
}
Example #3
0
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];
}
Example #4
0
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;
}
Example #5
0
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);
}
Example #6
0
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;
}
Example #7
0
void
cpSegmentShapeSetRadius(cpShape *shape, cpFloat radius)
{
	cpAssert(shape->klass == &cpSegmentShapeClass, "Shape is not a segment shape.");
	cpSegmentShape *seg = (cpSegmentShape *)shape;
	
	seg->r = radius;
}
Example #8
0
void
cpCircleShapeSetOffset(cpShape *shape, cpVect offset)
{
	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
	cpCircleShape *circle = (cpCircleShape *)shape;
	
	circle->c = offset;
}
Example #9
0
void
cpCircleShapeSetRadius(cpShape *shape, cpFloat radius)
{
	cpAssert(shape->klass == &cpCircleShapeClass, "Shape is not a circle shape.");
	cpCircleShape *circle = (cpCircleShape *)shape;
	
	circle->r = radius;
}
Example #10
0
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);
}
Example #11
0
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);
}
Example #12
0
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;
}
Example #13
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;
}
Example #14
0
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)));
}
Example #15
0
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);
}
Example #16
0
cpConstraint *
cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
{
	cpAssert(!cpArrayContains(space->constraints, constraint), "Cannot add the same constraint more than once.");
//	cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback.
	
	cpArrayPush(space->constraints, constraint);
	
	return constraint;
}
Example #17
0
cpPolyShape *
cpPolyShapeInit(cpPolyShape *poly, cpBody *body, int numVerts, cpVect *verts, cpVect offset)
{
	// Fail if the user attempts to pass a concave poly, or a bad winding.
	cpAssert(cpPolyValidate(verts, numVerts), "Polygon is concave or has a reversed winding.");

	setUpVerts(poly, numVerts, verts, offset);
	cpShapeInit((cpShape *)poly, &polyClass, body);

	return poly;
}
Example #18
0
cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
	cpAssertWarn(body->m != INFINITY, "Did you really mean to add an infinite mass body to the space?");
	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;
}
Example #19
0
cpBody *
cpSpaceAddBody(cpSpace *space, cpBody *body)
{
	cpAssertWarn(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated.");
	cpAssert(!body->space, "Cannot add a body to a more than one space or to the same space twice.");
//	cpAssertSpaceUnlocked(space); This should be safe as long as it's not from an integration callback
	
	cpArrayPush(space->bodies, body);
	body->space = space;
	
	return body;
}
Example #20
0
static void
cpBodyRemoveShape(cpBody *body, cpShape *shape)
{
	cpShape **prev_ptr = &body->shapesList;
	cpShape *node = body->shapesList;
	
	while(node && node != shape){
		prev_ptr = &node->next;
		node = node->next;
	}
	
	cpAssert(node, "Attempted to remove a shape from a body it was never attached to.");
	(*prev_ptr) = node->next;
}
Example #21
0
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
//	cpAssert(shape->body, "Cannot add a static shape with a NULL body.");
	if(!shape->body) shape->body = &space->staticBody;
	
	cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
		"Cannot add the same static shape more than once.");
	cpAssertSpaceUnlocked(space);
	
	cpShapeCacheBB(shape);
	cpSpaceHashInsert(space->staticShapes, shape, shape->hashid, shape->bb);
	
	return shape;
}
Example #22
0
cpConstraint *
cpSpaceAddConstraint(cpSpace *space, cpConstraint *constraint)
{
	cpAssert(!cpArrayContains(space->constraints, constraint), "Cannot add the same constraint more than once.");
//	cpAssertSpaceUnlocked(space); This should be safe as long as its not from a constraint callback.
	
	if(!constraint->a) constraint->a = &space->staticBody;
	if(!constraint->b) constraint->b = &space->staticBody;
	
	cpBodyActivate(constraint->a);
	cpBodyActivate(constraint->b);
	cpArrayPush(space->constraints, constraint);
	
	return constraint;
}
cpShape *
cpSpaceAddStaticShape(cpSpace *space, cpShape *shape)
{
	cpAssert(!cpHashSetFind(space->staticShapes->handleSet, shape->hashid, shape),
		"Cannot add the same static shape more than once.");
	cpAssertSpaceUnlocked(space);
	
	if(!shape->body) shape->body = &space->staticBody;
	
	cpShapeCacheBB(shape);
	activateShapesTouchingShape(space, shape);
	addShapeRaw(shape, space->staticShapes);
	
	return shape;
}
Example #24
0
// Transformation function for contactSet.
static void *
contactSetTrans(cpShape **shapes, cpSpace *space)
{
	if(space->pooledArbiters->num == 0){
		// arbiter pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(cpArbiter);
		cpAssert(count, "Buffer size too small.");
		
		cpArbiter *buffer = (cpArbiter *)cpmalloc(CP_BUFFER_BYTES);
		cpArrayPush(space->allocatedBuffers, buffer);
		
		for(int i=0; i<count; i++) cpArrayPush(space->pooledArbiters, buffer + i);
	}
	
	return cpArbiterInit((cpArbiter *) cpArrayPop(space->pooledArbiters), shapes[0], shapes[1]);
}
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
	cpBody *body = shape->body;
	if(!body || body == &space->staticBody) return cpSpaceAddStaticShape(space, shape);
	
	cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
		"Cannot add the same shape more than once.");
	cpAssertSpaceUnlocked(space);
	
	cpBodyActivate(body);
	cpBodyAddShape(body, shape);
	addShapeRaw(shape, space->activeShapes);
		
	return shape;
}
Example #26
0
cpShape *
cpSpaceAddShape(cpSpace *space, cpShape *shape)
{
	cpBody *body = shape->body;
	if(!body || cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape);
	
	cpAssert(!cpHashSetFind(space->activeShapes->handleSet, shape->hashid, shape),
		"Cannot add the same shape more than once.");
	cpAssertSpaceUnlocked(space);
	
	cpBodyActivate(body);
	cpBodyAddShape(body, shape);
	
	cpShapeCacheBB(shape);
	cpSpaceHashInsert(space->activeShapes, shape, shape->hashid, shape->bb);
		
	return shape;
}
// Transformation function for the handleset.
static void *
handleSetTrans(void *obj, cpSpaceHash *hash)
{
	if(hash->pooledHandles->num == 0){
		// handle pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(cpHandle);
		cpAssert(count, "Buffer size is too small.");
		
		cpHandle *buffer = (cpHandle *)cpmalloc(CP_BUFFER_BYTES);
		cpArrayPush(hash->allocatedBuffers, buffer);
		
		for(int i=0; i<count; i++) cpArrayPush(hash->pooledHandles, buffer + i);
	}
	
	cpHandle *hand = cpHandleInit((cpHandle *) cpArrayPop(hash->pooledHandles), obj);
	cpHandleRetain(hand);
	
	return hand;
}
Example #28
0
static inline void
componentActivate(cpBody *root)
{
	if(!cpBodyIsSleeping(root)) return;

	cpSpace *space = root->space;
	cpAssert(space, "Trying to activate a body that was never added to a space.");

	cpBody *body = root, *next;
	do {
		next = body->node.next;

		cpComponentNode node = {NULL, NULL, 0, 0.0f};
		body->node = node;

		cpSpaceActivateBody(space, body);
	} while((body = next) != root);

	cpArrayDeleteObj(space->sleepingComponents, root);
}
// Get a recycled or new bin.
static inline cpSpaceHashBin *
getEmptyBin(cpSpaceHash *hash)
{
	cpSpaceHashBin *bin = hash->pooledBins;
	
	if(bin){
		hash->pooledBins = bin->next;
		return bin;
	} else {
		// Pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(cpSpaceHashBin);
		cpAssert(count, "Buffer size is too small.");
		
		cpSpaceHashBin *buffer = (cpSpaceHashBin *)cpmalloc(CP_BUFFER_BYTES);
		cpArrayPush(hash->allocatedBuffers, buffer);
		
		// push all but the first one, return the first instead
		for(int i=1; i<count; i++) recycleBin(hash, buffer + i);
		return buffer;
	}
}
Example #30
0
static cpHashSetBin *
getUnusedBin(cpHashSet *set)
{
	cpHashSetBin *bin = set->pooledBins;
	
	if(bin){
		set->pooledBins = bin->next;
		return bin;
	} else {
		// Pool is exhausted, make more
		int count = CP_BUFFER_BYTES/sizeof(cpHashSetBin);
		cpAssert(count, "Buffer size is too small.");
		
		cpHashSetBin *buffer = (cpHashSetBin *)cpcalloc(1, CP_BUFFER_BYTES);
		cpArrayPush(set->allocatedBuffers, buffer);
		
		// push all but the first one, return the first instead
		for(int i=1; i<count; i++) recycleBin(set, buffer + i);
		return buffer;
	}
}