void b2PolygonAndCircleContact::Evaluate() { b2Body* bodyA = m_fixtureA->GetBody(); b2Body* bodyB = m_fixtureB->GetBody(); b2CollidePolygonAndCircle( &m_manifold, (b2PolygonShape*)m_fixtureA->GetShape(), bodyA->GetTransform(), (b2CircleShape*)m_fixtureB->GetShape(), bodyB->GetTransform()); }
void b2PolygonAndCircleContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB) { b2CollidePolygonAndCircle( manifold, (b2PolygonShape*)m_fixtureA->GetShape(), xfA, (b2CircleShape*)m_fixtureB->GetShape(), xfB); }
bool IsBlocked( const BoundingBox& bbox ) { b2AABB physBounds; physBounds.lowerBound = b2Vec2( bbox.Min.X, bbox.Min.Y ); physBounds.upperBound = b2Vec2( bbox.Max.X, bbox.Max.Y ); __spatialGraphNumFixtures = 0; theWorld.GetPhysicsWorld().QueryAABB(&theSpatialGraph, physBounds); //No bodies here if( __spatialGraphNumFixtures == 0 ) return false; b2PolygonShape shapeBoundsPoly; b2Vec2 vertices[4]; vertices[0].Set( physBounds.lowerBound.x, physBounds.lowerBound.y ); vertices[1].Set( physBounds.upperBound.x, physBounds.lowerBound.y ); vertices[2].Set( physBounds.upperBound.x, physBounds.upperBound.y ); vertices[3].Set( physBounds.lowerBound.x, physBounds.upperBound.y ); shapeBoundsPoly.Set(vertices, 4); b2BodyDef fakeBodyDef; //b2Vec2 center = physBounds.lowerBound + (0.5f * shapeBoundsDef.extents); fakeBodyDef.position.Set(0.0f, 0.0f ); b2Body* fakeBody = theWorld.GetPhysicsWorld().CreateBody(&fakeBodyDef); b2FixtureDef fakeFixtureDef; fakeFixtureDef.shape = &shapeBoundsPoly; b2Fixture* shapeBounds = fakeBody->CreateFixture(&fakeFixtureDef); for( int i = 0; i < __spatialGraphNumFixtures; i++ ) { b2Fixture* pFix = __spatialGraphTempFixtures[i]; if( pFix->GetType() == b2Shape::e_polygon ) { b2PolygonShape* pPolyShape = (b2PolygonShape*)pFix->GetShape(); b2Manifold m0; b2CollidePolygons(&m0, (b2PolygonShape*)shapeBounds->GetShape(), fakeBody->GetTransform(), pPolyShape, pFix->GetBody()->GetTransform()); if( m0.pointCount > 0 ) { theWorld.GetPhysicsWorld().DestroyBody(fakeBody); return true; } } else if( pFix->GetType() == b2Shape::e_circle ) { b2CircleShape* pCircleShape = (b2CircleShape*)pFix->GetShape(); b2Manifold m0; b2CollidePolygonAndCircle( &m0, (b2PolygonShape*)shapeBounds->GetShape(), fakeBody->GetTransform(), pCircleShape, pFix->GetBody()->GetTransform()); if( m0.pointCount > 0 ) { theWorld.GetPhysicsWorld().DestroyBody(fakeBody); return true; } } } theWorld.GetPhysicsWorld().DestroyBody(fakeBody); return false; }
bool wyBox2DCollisionDetector::isCollided(wyNode* node, wyRect& r, wyBox2DCDResult* result) { // init transform 2 sRectTrans2.SetIdentity(); b2Vec2 v; v.x = pixel2Meter(r.x + r.width / 2); v.y = pixel2Meter(r.y + r.height / 2); sRectTrans2.Set(v, 0); // init shape 2 sRectPoly2.SetAsBox(pixel2Meter(r.width) / 2, pixel2Meter(r.height) / 2); // get node hash, if not, add it wyNodeHash* hash = (wyNodeHash*)wyHashSetFind(m_nodeShapes, (size_t)node, node); if(hash == NULL) hash = addNode(node); // update node1 transform wyPoint pos = node->nodeToWorldSpace(wyp(node->getWidth() / 2, node->getHeight() / 2)); v.x = pixel2Meter(pos.x); v.y = pixel2Meter(pos.y); float angle = -wyMath::d2r(node->getRotation()); hash->transform.Set(v, angle); // collision detection bool reverseNormal = false; b2Manifold manifold; switch(hash->type) { case b2Shape::e_polygon: b2CollidePolygons(&manifold, &hash->poly, hash->transform, &sRectPoly2, sRectTrans2); break; case b2Shape::e_circle: b2CollidePolygonAndCircle(&manifold, &sRectPoly2, sRectTrans2, &hash->circle, hash->transform); reverseNormal = true; break; } // to world coordinates if(manifold.pointCount > 0) { // write data back if(result) { // convert to world coordinate b2WorldManifold worldManifold; if(reverseNormal) { worldManifold.Initialize(&manifold, sRectTrans2, sRectPoly2.m_radius, hash->transform, hash->type == b2Shape::e_polygon ? hash->poly.m_radius : hash->circle.m_radius); } else { worldManifold.Initialize(&manifold, hash->transform, hash->type == b2Shape::e_polygon ? hash->poly.m_radius : hash->circle.m_radius, sRectTrans2, sRectPoly2.m_radius); } // save contact points result->pointCount = manifold.pointCount; for(int i = 0; i < manifold.pointCount; i++) { result->points[i] = wyp(meter2Pixel(worldManifold.points[i].x), meter2Pixel(worldManifold.points[i].y)); } // save normal if(reverseNormal) result->normal = wyp(-worldManifold.normal.x, -worldManifold.normal.y); else result->normal = wyp(worldManifold.normal.x, worldManifold.normal.y); } } return manifold.pointCount > 0; }
void b2PolyAndCircleContact::Evaluate(b2ContactListener* listener) { b2Body* b1 = m_shape1->GetBody(); b2Body* b2 = m_shape2->GetBody(); b2Manifold m0; memcpy(&m0, &m_manifold, sizeof(b2Manifold)); b2CollidePolygonAndCircle(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2CircleShape*)m_shape2, b2->GetXForm()); bool persisted[b2_maxManifoldPoints] = {false, false}; b2ContactPoint cp; cp.shape1 = m_shape1; cp.shape2 = m_shape2; cp.friction = b2MixFriction(m_shape1->GetFriction(), m_shape2->GetFriction()); cp.restitution = b2MixRestitution(m_shape1->GetRestitution(), m_shape2->GetRestitution()); // Match contact ids to facilitate warm starting. if (m_manifold.pointCount > 0) { // Match old contact ids to new contact ids and copy the // stored impulses to warm start the solver. for (int32 i = 0; i < m_manifold.pointCount; ++i) { b2ManifoldPoint* mp = m_manifold.points + i; mp->normalImpulse = 0.0f; mp->tangentImpulse = 0.0f; bool found = false; b2ContactID id = mp->id; for (int32 j = 0; j < m0.pointCount; ++j) { if (persisted[j] == true) { continue; } b2ManifoldPoint* mp0 = m0.points + j; if (mp0->id.key == id.key) { persisted[j] = true; mp->normalImpulse = mp0->normalImpulse; mp->tangentImpulse = mp0->tangentImpulse; // A persistent point. found = true; // Report persistent point. if (listener != NULL) { cp.position = b1->GetWorldPoint(mp->localPoint1); b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1); b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2); cp.velocity = v2 - v1; cp.normal = m_manifold.normal; cp.separation = mp->separation; cp.id = id; listener->Persist(&cp); } break; } } // Report added point. if (found == false && listener != NULL) { cp.position = b1->GetWorldPoint(mp->localPoint1); b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp->localPoint1); b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp->localPoint2); cp.velocity = v2 - v1; cp.normal = m_manifold.normal; cp.separation = mp->separation; cp.id = id; listener->Add(&cp); } } m_manifoldCount = 1; } else { m_manifoldCount = 0; } if (listener == NULL) { return; } // Report removed points. for (int32 i = 0; i < m0.pointCount; ++i) { if (persisted[i]) { continue; } b2ManifoldPoint* mp0 = m0.points + i; cp.position = b1->GetWorldPoint(mp0->localPoint1); b2Vec2 v1 = b1->GetLinearVelocityFromLocalPoint(mp0->localPoint1); b2Vec2 v2 = b2->GetLinearVelocityFromLocalPoint(mp0->localPoint2); cp.velocity = v2 - v1; cp.normal = m0.normal; cp.separation = mp0->separation; cp.id = mp0->id; listener->Remove(&cp); } }