// Hashset filter func to throw away old arbiters. static cpBool cpSpaceArbiterSetFilter(cpArbiter *arb, cpSpace *space) { cpTimestamp ticks = space->stamp - arb->stamp; cpBody *a = arb->body_a, *b = arb->body_b; // TODO should make an arbiter state for this so it doesn't require filtering arbiters for dangling body pointers on body removal. // Preserve arbiters on sensors and rejected arbiters for sleeping objects. if( (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && (cpBodyIsStatic(b) || cpBodyIsSleeping(b)) ){ return cpTrue; } // Arbiter was used last frame, but not this one if(ticks >= 1 && arb->state != cpArbiterStateCached){ cpArbiterCallSeparate(arb, space); arb->state = cpArbiterStateCached; } if(ticks >= space->collisionPersistence){ arb->contacts = NULL; arb->numContacts = 0; cpArrayPush(space->pooledArbiters, arb); return cpFalse; } return cpTrue; }
static inline void mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b) { // Ignore connections to static bodies if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return; cpBody *a_root = componentNodeRoot(a); cpBody *b_root = componentNodeRoot(b); cpBool a_sleep = cpBodyIsSleeping(a_root); cpBool b_sleep = cpBodyIsSleeping(b_root); if(a_sleep && b_sleep){ return; } else if(a_sleep || b_sleep){ componentActivate(a_root); componentActivate(b_root); } // Add any rogue bodies found to the list and reset the idle time of anything they touch. if(cpBodyIsRogue(a)){ cpArrayPush(rogueBodies, a); b->node.idleTime = 0.0f; } if(cpBodyIsRogue(b)){ cpArrayPush(rogueBodies, b); a->node.idleTime = 0.0f; } componentNodeMerge(a_root, b_root); }
static inline void mergeBodies(cpSpace *space, cpArray *components, cpArray *rogueBodies, cpBody *a, cpBody *b) { // Don't merge with the static body if(cpBodyIsStatic(a) || cpBodyIsStatic(b)) return; cpBody *a_root = componentNodeRoot(a); cpBody *b_root = componentNodeRoot(b); cpBool a_sleep = cpBodyIsSleeping(a_root); cpBool b_sleep = cpBodyIsSleeping(b_root); if(a_sleep && b_sleep){ return; } else if(a_sleep || b_sleep){ componentActivate(a_root); componentActivate(b_root); } // Add any rogue bodies (bodies not added to the space) if(!a->space) cpArrayPush(rogueBodies, a); if(!b->space) cpArrayPush(rogueBodies, b); componentNodeMerge(a_root, b_root); }
__declspec( dllexport ) void pullall( const void * _in, int in_size, void * _out, int out_sz ) { int i; cpBody *body; Variable *var; //int bodyCount = 0; int index = 0; for (i = 0;i != mVariableHandler.mSize;i++) { var = mVariableHandler.mPtrArray[i]; if (var == NULL)continue; //Varmistetaan, että haetaan vain runkojen tiedot if (var->mType != VarTypeBody)continue; body = (cpBody*)var->mPtr; //Eikä staattisen tietoja if (cpBodyIsStatic(body))continue; //Alkuun cb:n muistipalan osoite POKEINT(OUTPUT_MEMBLOCK,index,var->mCBPtr); POKEFLOAT(OUTPUT_MEMBLOCK,index+4,radToDeg(cpBodyGetAngle(body))); POKEVECT(OUTPUT_MEMBLOCK,index+8,cpBodyGetPos(body)); POKEFLOAT(OUTPUT_MEMBLOCK,index+16,radToDeg(cpBodyGetAngVel(body))); POKEVECT(OUTPUT_MEMBLOCK,index+20,cpBodyGetVel(body)); index += 28; //bodyCount++; } //POKEINT(OUTPUT_MEMBLOCK,0,bodyCount); }
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); }
static unsigned int _shape_add(Entity ent, PhysicsShape type, cpShape *shape) { PhysicsInfo *info; ShapeInfo *shapeInfo; info = entitypool_get(pool, ent); error_assert(info); /* init ShapeInfo */ shapeInfo = array_add(info->shapes); shapeInfo->type = type; shapeInfo->shape = shape; /* init cpShape */ cpShapeSetBody(shape, info->body); cpSpaceAddShape(space, shape); cpShapeSetFriction(shapeInfo->shape, 1); cpShapeSetUserData(shapeInfo->shape, ent); /* update moment */ if (!cpBodyIsStatic(info->body)) { if (array_length(info->shapes) > 1) cpBodySetMoment(info->body, _moment(info->body, shapeInfo) + cpBodyGetMoment(info->body)); else cpBodySetMoment(info->body, _moment(info->body, shapeInfo)); } return array_length(info->shapes) - 1; }
static void _set_type(PhysicsInfo *info, PhysicsBody type) { if (info->type == type) return; /* already set */ info->type = type; switch (type) { case PB_KINEMATIC: info->last_pos = cpBodyGetPos(info->body); info->last_ang = cpBodyGetAngle(info->body); /* fall through */ case PB_STATIC: if (!cpBodyIsStatic(info->body)) { cpSpaceRemoveBody(space, info->body); cpSpaceConvertBodyToStatic(space, info->body); } break; case PB_DYNAMIC: cpSpaceConvertBodyToDynamic(space, info->body, info->mass, 1.0); cpSpaceAddBody(space, info->body); _recalculate_moment(info); break; } }
__declspec( dllexport ) void explosion( const void * _in, int in_size, void * _out, int out_sz ) { int i; Variable *var; cpBody *body; cpVect bp; float angle; float dist; float divi; float finalPower; float power = PEEKFLOAT(INPUT_MEMBLOCK,0); cpVect position = PEEKVECT(INPUT_MEMBLOCK,4); for (i = 0;i != mVariableHandler.mSize;i++) { var = mVariableHandler.mPtrArray[i]; if (var == NULL)continue; if (var->mType != VarTypeBody) continue; body = (cpBody*)var->mPtr; if (cpBodyIsStatic(body)) continue; bp = cpvsub(position,cpBodyGetPos(body)); dist = cpvlength(bp)+0.1f; divi = (power/dist); finalPower = -min(power,divi*divi); angle = cpvtoangle(bp); cpBodyApplyImpulse(body,cpv(cosf(angle)*finalPower,sinf(angle)*finalPower),cpBodyWorld2Local(body,position)); } }
void Unit::setUnitPos( float x, float y ) { cpVect v = {x, y}; cpBodySetPos( physBody, v ); Image.setPosition( x, y, Z ); if( cpBodyIsStatic(physBody) && phys.type != potNone ) cpSpaceReindexShape( Phys::space, physShape ); }
static VALUE rb_cpBodyIsStatic(VALUE self) { cpBody * body = BODY(self); cpBool stat = 0; // cpBodyInitStatic(body); stat = cpBodyIsStatic(body); return stat ? Qtrue : Qfalse; // }
// Hashset filter func to throw away old arbiters. static cpBool contactSetFilter(cpArbiter *arb, cpSpace *space) { if(space->sleepTimeThreshold != INFINITY){ cpBody *a = arb->a->body; cpBody *b = arb->b->body; // both bodies are either static or sleeping cpBool sleepingNow = (cpBodyIsStatic(a) || cpBodyIsSleeping(a)) && (cpBodyIsStatic(b) || cpBodyIsSleeping(b)); if(sleepingNow){ arb->state = cpArbiterStateSleep; return cpTrue; } else if(arb->state == cpArbiterStateSleep){ // wake up the arbiter and continue as normal arb->state = cpArbiterStateNormal; // TODO is it possible that cpArbiterStateIgnore should be set here instead? } } cpTimestamp ticks = space->stamp - arb->stamp; // was used last frame, but not this one if(ticks >= 1 && arb->state != cpArbiterStateCached){ // The handler needs to be looked up again as the handler cached on the arbiter may have been deleted since the last step. cpCollisionHandler *handler = lookupCollisionHandler(space, arb->a->collision_type, arb->b->collision_type); handler->separate(arb, space, handler->data); arb->state = cpArbiterStateCached; } if(ticks >= cp_contact_persistence){ arb->contacts = NULL; arb->numContacts = 0; cpArrayPush(space->pooledArbiters, arb); return cpFalse; } return cpTrue; }
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; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsStatic(body), "Static bodies cannot be added to a space as they are not meant to be simulated."); cpAssertSoft(!body->space, "This body is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpArrayPush(space->bodies, body); body->space = space; return body; }
cpBody * cpSpaceAddBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsStatic(body), "Do not add static bodies to a space. Static bodies do not move and should not be simulated."); cpAssertHard(body->space != space, "You have already added this body to this space. You must not add it a second time."); cpAssertHard(!body->space, "You have already added this body to another space. You cannot add it to a second."); cpAssertSpaceUnlocked(space); cpArrayPush(space->bodies, body); body->space = space; return body; }
void PhysicsWorld::addShape(PhysicsShape* shape) { for (auto it = shape->_info->shapes.begin(); it != shape->_info->shapes.end(); it++) { if (cpBodyIsStatic(shape->getBody()->_info->body)) { cpSpaceAddStaticShape(_info->space, *it); }else { cpSpaceAddShape(_info->space, *it); } } }
void cpSpaceRemoveStaticShape(cpSpace *space, cpShape *shape) { cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a static or sleeping shape that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); cpBody *body = shape->body; if(cpBodyIsStatic(body)) cpBodyActivateStatic(body, shape); cpBodyRemoveShape(body, shape); cpSpaceFilterArbiters(space, body, shape); cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); shape->space = NULL; }
CDynamics2DSingleBodyObjectModel::~CDynamics2DSingleBodyObjectModel() { bool bIsStatic = cpBodyIsStatic(m_ptBody); /* Dispose of shapes */ for(cpShape* pt_shape = m_ptBody->shapeList; pt_shape != NULL; pt_shape = pt_shape->next) { cpSpaceRemoveShape(GetDynamics2DEngine().GetPhysicsSpace(), pt_shape); cpShapeFree(pt_shape); } /* Dispose of body */ if(! bIsStatic) cpSpaceRemoveBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody); cpBodyFree(m_ptBody); /* Reindex space */ if(bIsStatic) cpSpaceReindexStatic(GetDynamics2DEngine().GetPhysicsSpace()); }
void cpSpaceActivateBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rogue body."); if(space->locked){ // cpSpaceActivateBody() is called again once the space is unlocked if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body); } else { cpAssertSoft(body->node.root == NULL && body->node.next == NULL, "Internal error: Activating body non-NULL node pointers."); cpArrayPush(space->bodies, body); CP_BODY_FOREACH_SHAPE(body, shape){ cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); } CP_BODY_FOREACH_ARBITER(body, arb){ cpBody *bodyA = arb->body_a; // Arbiters are shared between two bodies that are always woken up together. // You only want to restore the arbiter once, so bodyA is arbitrarily chosen to own the arbiter. // The edge case is when static bodies are involved as the static bodies never actually sleep. // If the static body is bodyB then all is good. If the static body is bodyA, that can easily be checked. if(body == bodyA || cpBodyIsStatic(bodyA)){ int numContacts = arb->numContacts; cpContact *contacts = arb->contacts; // Restore contact values back to the space's contact buffer memory arb->contacts = cpContactBufferGetArray(space); memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact)); cpSpacePushContacts(space, numContacts); // Reinsert the arbiter into the arbiter cache cpShape *a = arb->a, *b = arb->b; cpShape *shape_pair[] = {a, b}; cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL); // Update the arbiter's state arb->stamp = space->stamp; arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type); cpArrayPush(space->arbiters, arb); cpfree(contacts); } }
void Unit::updatePhysics( ) { cpBodySetMass( physBody, phys.mass ); cpFloat moment; switch(phys.type){ case potCircle: if( physShape ) cpCircleShapeSetRadius( physShape, phys.radius ); moment = cpMomentForCircle( phys.mass, 0, phys.radius, cpvzero ); break; default: moment = 1.0; break; } cpBodySetMoment( physBody, moment ); if( cpBodyIsStatic(physBody) && phys.type != potNone ) cpSpaceReindexShape( Phys::space, physShape ); }
//MARK: Body, Shape, and Joint Management cpShape * cpSpaceAddShape(cpSpace *space, cpShape *shape) { cpBody *body = shape->body; if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape); cpAssertHard(!shape->space, "This shape is already added to a space and cannot be added to another."); cpAssertSpaceUnlocked(space); cpBodyActivate(body); cpBodyAddShape(body, shape); cpShapeUpdate(shape, body->p, body->rot); cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); shape->space = space; return shape; }
void cpSpaceConvertBodyToStatic(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsStatic(body), "Body is already static."); cpAssertHard(cpBodyIsRogue(body), "Remove the body from the space before calling this function."); cpAssertSpaceUnlocked(space); cpBodySetMass(body, INFINITY); cpBodySetMoment(body, INFINITY); cpBodySetVel(body, cpvzero); cpBodySetAngVel(body, 0.0f); body->node.idleTime = INFINITY; CP_BODY_FOREACH_SHAPE(body, shape){ cpSpatialIndexRemove(space->activeShapes, shape, shape->hashid); cpSpatialIndexInsert(space->staticShapes, shape, shape->hashid); }
static cpBody * rb_cpBodySleepValidate(VALUE vbody) { cpBody * body = BODY(vbody); cpSpace *space = body->CP_PRIVATE(space); if(!space) { rb_raise(rb_eArgError, "Cannot put a body to sleep that has not been added to a space."); return NULL; } if (cpBodyIsStatic(body) && cpBodyIsRogue(body)) { rb_raise(rb_eArgError, "Rogue AND static bodies cannot be put to sleep."); return NULL; } if(cpSpaceIsLocked(space)) { rb_raise(rb_eArgError, "Bodies can not be put to sleep during a query or a call to Space#add_collision_func. Put these calls into a post-step callback using Space#add_collision_handler."); return NULL; } return body; }
void CDynamics2DSingleBodyObjectModel::Reset() { /* Nothing to do for a static body */ if(cpBodyIsStatic(m_ptBody)) return; /* Reset body position */ const CVector3& cPosition = GetEmbodiedEntity().GetOriginAnchor().Position; m_ptBody->p = cpv(cPosition.GetX(), cPosition.GetY()); /* Reset body orientation */ CRadians cXAngle, cYAngle, cZAngle; GetEmbodiedEntity().GetOriginAnchor().Orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle); cpBodySetAngle(m_ptBody, cZAngle.GetValue()); /* Zero speed and applied forces */ m_ptBody->v = cpvzero; m_ptBody->w = 0.0f; cpBodyResetForces(m_ptBody); /* Update bounding box */ cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody); CalculateBoundingBox(); }
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; }
void CDynamics2DSingleBodyObjectModel::MoveTo(const CVector3& c_position, const CQuaternion& c_orientation) { /* Move the body to the desired position */ m_ptBody->p = cpv(c_position.GetX(), c_position.GetY()); CRadians cXAngle, cYAngle, cZAngle; c_orientation.ToEulerAngles(cZAngle, cYAngle, cXAngle); cpBodySetAngle(m_ptBody, cZAngle.GetValue()); /* Update shape index */ if(cpBodyIsStatic(m_ptBody)) { cpBB tBoundingBox = cpShapeGetBB(m_ptBody->shapeList); cpSpaceReindexStatic(GetDynamics2DEngine().GetPhysicsSpace()); tBoundingBox = cpShapeGetBB(m_ptBody->shapeList); } else { cpSpaceReindexShapesForBody(GetDynamics2DEngine().GetPhysicsSpace(), m_ptBody); } /* Update ARGoS entity state */ CDynamics2DModel::UpdateEntityStatus(); }
//MARK: Body, Shape, and Joint Management cpShape * cpSpaceAddShape(cpSpace *space, cpShape *shape) { cpBody *body = shape->body; if(cpBodyIsStatic(body)) return cpSpaceAddStaticShape(space, shape); cpAssertHard(shape->space != space, "You have already added this shape to this space. You must not add it a second time."); cpAssertHard(!shape->space, "You have already added this shape to another space. You cannot add it to a second."); cpAssertSpaceUnlocked(space); cpBodyActivate(body); cpBodyAddShape(body, shape); cpShapeUpdate(shape, body->p, body->rot); cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); shape->space = space; return shape; }
Unit::~Unit() { ActionTimer* timer = NULL; while( actionTimers != NULL ){ timer = actionTimers; Timer::DeleteTimerEventById( timer->timerId ); actionTimers = timer->next; delete timer, timer = NULL; } if( physShape ){ cpSpaceRemoveShape( Phys::space, physShape ); cpShapeFree( physShape ); } if( physBody ){ if( !cpBodyIsStatic(physBody) ) cpSpaceRemoveBody( Phys::space, physBody ); cpBodyFree( physBody ); } }
static void update(cpSpace *space, double dt) { if(ChipmunkDemoRightDown){ cpShape *nearest = cpSpaceNearestPointQueryNearest(space, ChipmunkDemoMouse, 0.0, GRABABLE_MASK_BIT, CP_NO_GROUP, NULL); if(nearest){ cpBody *body = cpShapeGetBody(nearest); if(cpBodyIsStatic(body)){ cpSpaceConvertBodyToDynamic(space, body, pentagon_mass, pentagon_moment); cpSpaceAddBody(space, body); } else { cpSpaceRemoveBody(space, body); cpSpaceConvertBodyToStatic(space, body); } } } cpSpaceStep(space, dt); cpSpaceEachBody(space, &eachBody, NULL); }
void cpSpaceRemoveShape(cpSpace *space, cpShape *shape) { cpBody *body = shape->body; cpAssertHard(cpSpaceContainsShape(space, shape), "Cannot remove a shape that was not added to the space. (Removed twice maybe?)"); cpAssertSpaceUnlocked(space); cpBool isStatic = cpBodyIsStatic(body); if(isStatic){ cpBodyActivateStatic(body, shape); } else { cpBodyActivate(body); } cpBodyRemoveShape(body, shape); cpSpaceFilterArbiters(space, body, shape); cpSpatialIndexRemove(isStatic ? space->staticShapes : space->dynamicShapes, shape, shape->hashid); shape->space = NULL; shape->hashid = 0; }
void cpSpaceActivateBody(cpSpace *space, cpBody *body) { cpAssertHard(!cpBodyIsRogue(body), "Internal error: Attempting to activate a rouge body."); if(space->locked){ // cpSpaceActivateBody() is called again once the space is unlocked if(!cpArrayContains(space->rousedBodies, body)) cpArrayPush(space->rousedBodies, body); } else { cpArrayPush(space->bodies, body); CP_BODY_FOREACH_SHAPE(body, shape){ cpSpatialIndexRemove(space->staticShapes, shape, shape->hashid); cpSpatialIndexInsert(space->activeShapes, shape, shape->hashid); } CP_BODY_FOREACH_ARBITER(body, arb){ cpBody *bodyA = arb->body_a; if(body == bodyA || cpBodyIsStatic(bodyA)){ int numContacts = arb->numContacts; cpContact *contacts = arb->contacts; // Restore contact values back to the space's contact buffer memory arb->contacts = cpContactBufferGetArray(space); memcpy(arb->contacts, contacts, numContacts*sizeof(cpContact)); cpSpacePushContacts(space, numContacts); // Reinsert the arbiter into the arbiter cache cpShape *a = arb->a, *b = arb->b; cpShape *shape_pair[] = {a, b}; cpHashValue arbHashID = CP_HASH_PAIR((cpHashValue)a, (cpHashValue)b); cpHashSetInsert(space->cachedArbiters, arbHashID, shape_pair, arb, NULL); // Update the arbiter's state arb->stamp = space->stamp; arb->handler = cpSpaceLookupHandler(space, a->collision_type, b->collision_type); cpArrayPush(space->arbiters, arb); cpfree(contacts); } }