void Flatland::Intersection::FindCircleCircle(const Geometry &g1, const Geometry &g2, ContactList &contacts)
{
assert(g1.GetShape() == Shape::Circle);
assert(g2.GetShape() == Shape::Circle);
const Circle &c1 = static_cast<const Circle&>(g1);
const Circle &c2 = static_cast<const Circle&>(g2);
vec2 delta = c2.Center() - c1.Center();
float radiusSum = c1.Radius() + c2.Radius();
float distance = delta.length();
if (distance > radiusSum)
return;
vec2 pos;
vec2 normal;
float depth;
if (distance <= 0)
{
pos = c1.Center();
normal = vec2(1, 0);
depth = radiusSum;
}
else
{
normal = delta / distance;
float k = 0.5 * (c2.Radius() - c1.Radius() - distance);
pos = c1.Center() + normal * k;
depth = radiusSum - distance;
}
contacts.AddContact(pos, -normal, depth);
}
bool Flatland::Intersection::TestCircleCircle(const Geometry &g1, const Geometry &g2)
{
assert(g1.GetShape() == Shape::Circle);
assert(g2.GetShape() == Shape::Circle);
const Circle &c1 = static_cast<const Circle&>(g1);
const Circle &c2 = static_cast<const Circle&>(g2);
float distance = (c2.Center() - c1.Center()).length();
return distance <= c1.Radius() + c2.Radius();
}
bool TestCompositeGeometry(const Geometry &g1, const Geometry &g2)
{
assert(g1.GetShape() == Shape::Composite);
const Composite &c = static_cast<const Composite&>(g1);
for (Composite::const_iterator g = c.begin(); g != c.end(); ++g)
if (Intersection::Test(**g, g2))
return true;
return false;
}
void FindCompositeGeometry(const Geometry &g1, const Geometry &g2, ContactList &contacts)
{
assert(g1.GetShape() == Shape::Composite);
const Composite &c = static_cast<const Composite&>(g1);
for (Composite::const_iterator g = c.begin(); g != c.end(); ++g)
{
if (Intersection::Test(**g, g2))
Intersection::Find(**g, g2, contacts);
}
}
void FindTerrainGeometry(const Geometry &g1, const Geometry &g2, ContactList &contacts)
{
assert(g1.GetShape() == Shape::Terrain);
const Terrain &t = static_cast<const Terrain&>(g1);
const Geometry &g = g2;
for (int index = lower; index <= upper; ++index)
{
const Line &line = t[index];
if (Intersection::Test(line, g)) // TODO inefficient because it's already been called
Intersection::Find(line, g, contacts);
}
}
bool TestTerrainGeometry(const Geometry &g1, const Geometry &g2)
{
assert(g1.GetShape() == Shape::Terrain);
const Terrain &t = static_cast<const Terrain&>(g1);
const Geometry &g = g2;
if (!t.GetIndexRange(g.GetBounds().left, g.GetBounds().right, lower, upper))
return false;
for (int index = lower; index <= upper; ++index)
{
assert(index >= 0 && index < (int) t.size());
const Line &line = t[index];
if (Intersection::Test(line, g))
return true;
}
return false;
}