//#include <stdio.h>
void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionProcessInfo& processInfo)
{
if (!m_manifoldPtr)
return;
btBox2dShape* box0 = (btBox2dShape*)processInfo.m_body0.getCollisionShape();
btBox2dShape* box1 = (btBox2dShape*)processInfo.m_body1.getCollisionShape();
processInfo.m_result->setPersistentManifold(m_manifoldPtr);
b2CollidePolygons(
processInfo.m_result,
box0,
processInfo.m_body0.getWorldTransform(),
box1,
processInfo.m_body1.getWorldTransform()
);
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
processInfo.m_result->refreshContactPoints();
}
}
void b2PolygonContact::Collide()
{
b2Body* b1 = m_shape1->GetBody();
b2Body* b2 = m_shape2->GetBody();
b2CollidePolygons(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2PolygonShape*)m_shape2, b2->GetXForm());
}
void b2PolygonContact::Evaluate()
{
b2Body* bodyA = m_fixtureA->GetBody();
b2Body* bodyB = m_fixtureB->GetBody();
b2CollidePolygons( &m_manifold,
(b2PolygonShape*)m_fixtureA->GetShape(), bodyA->GetXForm(),
(b2PolygonShape*)m_fixtureB->GetShape(), bodyB->GetXForm());
}
// Polygon versus 2-sided edge.
void b2CollidePolyAndEdge(b2Manifold* manifold,
const b2PolygonShape* polygon,
const b2XForm& transformA,
const b2EdgeShape* edge,
const b2XForm& transformB)
{
b2PolygonShape polygonB;
polygonB.SetAsEdge(edge->m_v1, edge->m_v2);
b2CollidePolygons(manifold, polygon, transformA, &polygonB, transformB);
}
bool wyBox2DCollisionDetector::isCollided(wyRect& r1, wyRect& r2, wyBox2DCDResult* result) {
// init transform 1
sRectTrans1.SetIdentity();
b2Vec2 v;
v.x = pixel2Meter(r1.x + r1.width / 2);
v.y = pixel2Meter(r1.y + r1.height / 2);
sRectTrans1.Set(v, 0);
// init transform 2
sRectTrans2.SetIdentity();
v.x = pixel2Meter(r2.x + r2.width / 2);
v.y = pixel2Meter(r2.y + r2.height / 2);
sRectTrans2.Set(v, 0);
// init shape 1
sRectPoly1.SetAsBox(pixel2Meter(r1.width) / 2, pixel2Meter(r1.height) / 2);
// init shape 2
sRectPoly2.SetAsBox(pixel2Meter(r2.width) / 2, pixel2Meter(r2.height) / 2);
// collision detection
b2Manifold manifold;
b2CollidePolygons(&manifold, &sRectPoly1, sRectTrans1, &sRectPoly2, sRectTrans2);
// to world coordinates
if(manifold.pointCount > 0) {
// write data back
if(result) {
// convert to world coordinate
b2WorldManifold worldManifold;
worldManifold.Initialize(&manifold, sRectTrans1, sRectPoly1.m_radius, sRectTrans2, sRectPoly2.m_radius);
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));
}
result->normal = wyp(worldManifold.normal.x, worldManifold.normal.y);
}
}
return manifold.pointCount > 0;
}
//#include <stdio.h>
void btBox2dBox2dCollisionAlgorithm::processCollision (const btCollisionObjectWrapper* body0Wrap,const btCollisionObjectWrapper* body1Wrap,const btDispatcherInfo& dispatchInfo,btManifoldResult* resultOut)
{
if (!m_manifoldPtr)
return;
const btBox2dShape* box0 = (const btBox2dShape*)body0Wrap->getCollisionShape();
const btBox2dShape* box1 = (const btBox2dShape*)body1Wrap->getCollisionShape();
resultOut->setPersistentManifold(m_manifoldPtr);
b2CollidePolygons(resultOut,box0,body0Wrap->getWorldTransform(),box1,body1Wrap->getWorldTransform());
// refreshContactPoints is only necessary when using persistent contact points. otherwise all points are newly added
if (m_ownManifold)
{
resultOut->refreshContactPoints();
}
}
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;
}
void b2PolygonContact::Evaluate(b2Manifold* manifold, const b2Transform& xfA, const b2Transform& xfB)
{
b2CollidePolygons( manifold,
(b2PolygonShape*)m_fixtureA->GetShape(), xfA,
(b2PolygonShape*)m_fixtureB->GetShape(), xfB);
}
void b2PolygonContact::Evaluate(b2ContactListener* listener)
{
b2Body* b1 = m_shape1->GetBody();
b2Body* b2 = m_shape2->GetBody();
b2Manifold m0;
memcpy(&m0, &m_manifold, sizeof(b2Manifold));
b2CollidePolygons(&m_manifold, (b2PolygonShape*)m_shape1, b1->GetXForm(), (b2PolygonShape*)m_shape2, b2->GetXForm());
bool persisted[b2_maxManifoldPoints] = {false, false};
b2ContactPoint cp;
cp.shape1 = m_shape1;
cp.shape2 = m_shape2;
cp.friction = m_friction;
cp.restitution = m_restitution;
// 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);
}
}
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;
}
