Skip to content

Commit

Permalink
Updates RayCast code to handle the vertex-radius and removes RayCast …
Browse files Browse the repository at this point in the history 
…line-specialization code.
  • Loading branch information
@louis-langholtz
louis-langholtz committed Apr 30, 2017
1 parent 7716c4b commit a952a62
Show file tree
Hide file tree
Showing 5 changed files with 106 additions and 143 deletions.
217 changes: 85 additions & 132 deletions Box2D/Box2D/Collision/RayCastOutput.cpp
View file
Expand Up @@ -33,22 +33,21 @@ using namespace box2d;

namespace
{
inline RayCastOutput RayCast(const Length radius, const Length2D v0,
const RayCastInput& input, const Transformation& transform) noexcept
inline RayCastOutput RayCast(const Length radius, const Length2D location,
const RayCastInput& input) noexcept
{
// Collision Detection in Interactive 3D Environments by Gino van den Bergen
// From Section 3.1.2
// x = s + a * r
// norm(x) = radius

const auto position = transform.p + Rotate(v0, transform.q);
const auto s = input.p1 - position;
const auto s = input.p1 - location;
const auto sUnitless = StripUnits(s);
const auto b = GetLengthSquared(sUnitless) - Square(radius / Meter);

// Solve quadratic equation.
const auto r = input.p2 - input.p1;
const auto rUnitless = StripUnits(r);
const auto raySegment = input.p2 - input.p1;
const auto rUnitless = StripUnits(raySegment);
const auto c = Dot(sUnitless, rUnitless);
const auto rr = GetLengthSquared(rUnitless);
const auto sigma = Square(c) - rr * b;
Expand All @@ -61,11 +60,11 @@ namespace

// Find the point of intersection of the line with the circle.
const auto a = -(c + Sqrt(sigma));

const auto fraction = a / rr;

// Is the intersection point on the segment?
if ((a >= RealNum{0}) && (a <= (input.maxFraction * rr)))
if ((fraction >= RealNum{0}) && (fraction <= input.maxFraction))
{
const auto fraction = a / rr;
return RayCastOutput{
GetUnitVector(sUnitless + fraction * rUnitless, UnitVec2::GetZero()),
fraction
Expand All @@ -74,64 +73,6 @@ namespace

return RayCastOutput{};
}

inline RayCastOutput RayCast(const Length2D v1, const Length2D v2,
const UnitVec2 normal,
const RayCastInput& input, const Transformation& transform) noexcept
{
// p = p1 + t * d
// v = v1 + s * e
// p1 + t * d = v1 + s * e
// s * e - t * d = p1 - v1

// Put the ray into the edge's frame of reference.
const auto d1 = input.p1 - transform.p;
const auto p1 = InverseRotate(StripUnits(d1), transform.q);
const auto d2 = input.p2 - transform.p;
const auto p2 = InverseRotate(StripUnits(d2), transform.q);
const auto d = p2 - p1;

const auto e = v2 - v1;
const auto eUnitless = StripUnits(e);

// q = p1 + t * d
// dot(normal, q - v1) = 0
// dot(normal, p1 - v1) + t * dot(normal, d) = 0
const auto v1p1 = v1 - p1 * Meter;
const auto numerator = Dot(normal, StripUnits(v1p1));
const auto denominator = Dot(normal, d);

if (denominator == 0)
{
return RayCastOutput{};
}

const auto t = numerator / denominator;
if ((t < 0) || (t > input.maxFraction))
{
return RayCastOutput{};
}

const auto q = p1 + t * d;

// q = v1 + s * e
// s = dot(q - v1, e) / dot(e, e)
const auto ee = GetLengthSquared(eUnitless);
if (ee == 0)
{
return RayCastOutput{};
}

const auto qv1 = q * Meter - v1;
const auto s = Dot(StripUnits(qv1), eUnitless) / ee;
if ((s < 0) || (s > 1))
{
return RayCastOutput{};
}

const auto normalFound = (numerator > 0)? -normal: normal;
return RayCastOutput{Rotate(normalFound, transform.q), t};
}

} // anonymous namespace

Expand Down Expand Up @@ -211,83 +152,95 @@ RayCastOutput box2d::RayCast(const DistanceProxy& proxy, const RayCastInput& inp
{
const auto vertexCount = proxy.GetVertexCount();
assert(vertexCount > 0);
switch (vertexCount)

const auto radius = proxy.GetVertexRadius();
auto v0 = proxy.GetVertex(0);
if (vertexCount == 1)
{
case 0:
return RayCastOutput{};
case 1:
return ::RayCast(proxy.GetVertexRadius(), proxy.GetVertex(0), input, transform);
case 2:
return ::RayCast(proxy.GetVertex(0), proxy.GetVertex(1), proxy.GetNormal(0),
input, transform);
default:
return ::RayCast(radius, Transform(v0, transform), input);
}

// Uses algorithm described at http://stackoverflow.com/a/565282/7410358
//
// The SO author gave the algorithm the following credit:
// "Intersection of two lines in three-space" by Ronald Goldman,
// published in Graphics Gems, page 304.

// Solve for p + t r = q + u s

// p is input.p1
// q is the offset vertex
// s is vertexDelta
// r is rayDelta
// t = (q − p) × s / (r × s)
// u = (q − p) × r / (r × s)

// Put the ray into the polygon's frame of reference.
const auto transformedInput = RayCastInput{
InverseTransform(input.p1, transform),
InverseTransform(input.p2, transform),
input.maxFraction
};
const auto ray0 = transformedInput.p1;
const auto ray = transformedInput.p2 - transformedInput.p1; // Ray delta (p2 - p1)

auto minT = std::nextafter(input.maxFraction, RealNum(2));
auto normalFound = GetInvalid<UnitVec2>();

for (auto i = decltype(vertexCount){0}; i < vertexCount; ++i)
{
const auto circleResult = ::RayCast(radius, v0, transformedInput);
if (minT > circleResult.fraction)
{
// Put the ray into the polygon's frame of reference.
const auto p1 = InverseRotate(input.p1 - transform.p, transform.q);
const auto p2 = InverseRotate(input.p2 - transform.p, transform.q);
const auto d = StripUnits(p2 - p1);
minT = circleResult.fraction;
normalFound = circleResult.normal;
}

const auto v1 = proxy.GetVertex(GetModuloNext(i, vertexCount));
const auto edge = v1 - v0; // Vertex delta
const auto ray_cross_edge = Cross(ray, edge);

if (ray_cross_edge != Area{0})
{
const auto normal = proxy.GetNormal(i);
const auto offset = normal * radius;
const auto v0off = v0 + offset;
const auto q_sub_p = v0off - ray0;

auto lower = RealNum{0};
auto upper = input.maxFraction;
auto normalFound = GetInvalid<UnitVec2>();
// t = ((q − p) × s) / (r × s)
const auto t = Cross(q_sub_p, edge) / ray_cross_edge;

for (auto i = decltype(vertexCount){0}; i < vertexCount; ++i)
// u = ((q − p) × r) / (r × s)
const auto u = Cross(q_sub_p, ray) / ray_cross_edge;

if ((t >= RealNum(0)) && (t <= RealNum(1)) &&
(u >= RealNum(0)) && (u <= RealNum(1)))
{
// p = p1 + a * d
// dot(normal, p - v) = 0
// dot(normal, p1 - v) + a * dot(normal, d) = 0
const auto normal = proxy.GetNormal(i);
const auto vertex = proxy.GetVertex(i);
const auto numerator = Dot(normal, StripUnits(vertex - p1));
const auto denominator = Dot(normal, d);

if (denominator == RealNum{0})
// The two lines meet at the point p + t r = q + u s
if (minT > t)
{
if (numerator < RealNum{0})
{
return RayCastOutput{};
}
}
else
{
const auto t = numerator / denominator;

// Note: we want this predicate without division:
// lower < numerator / denominator, where denominator < 0
// Since denominator < 0, we have to flip the inequality:
// lower < numerator / denominator <==> denominator * lower > numerator.
if (denominator < RealNum{0} && numerator < lower * denominator)
{
// Increase lower. The segment enters this half-space.
lower = t;
normalFound = normal;
}
else if (denominator > RealNum{0} && numerator < upper * denominator)
{
// Decrease upper. The segment exits this half-space.
upper = t;
}
}

if (upper < lower)
{
if (!almost_equal(upper, lower))
{
return RayCastOutput{};
}
std::swap(upper, lower);
minT = t;
normalFound = normal;
}
}
assert(RealNum{0} <= lower);
assert(lower <= input.maxFraction);

if (IsValid(normalFound))
else
{
return RayCastOutput{Rotate(normalFound, transform.q), lower};
// The two line segments are not parallel but do not intersect.
}
return RayCastOutput{};
}
else
{
// The two lines are parallel, igonred.
}

v0 = v1;
}

if (minT <= input.maxFraction)
{
return RayCastOutput{Rotate(normalFound, transform.q), minT};
}
return RayCastOutput{};
}

RayCastOutput box2d::RayCast(const Fixture& f, const RayCastInput& input, child_count_t childIndex)
Expand Down
4 changes: 2 additions & 2 deletions Box2D/Box2D/Collision/RayCastOutput.hpp
View file
Expand Up @@ -41,8 +41,8 @@ namespace box2d
// Intentionally empty.
}

UnitVec2 normal;
RealNum fraction = 0;
UnitVec2 normal = GetInvalid<decltype(normal)>();
RealNum fraction = GetInvalid<decltype(fraction)>();
bool hit = false;
};

Expand Down
1 change: 0 additions & 1 deletion Box2D/Box2D/Collision/Shapes/Shape.hpp
View file
Expand Up @@ -23,7 +23,6 @@
#include <Box2D/Common/Math.hpp>
#include <Box2D/Collision/DistanceProxy.hpp>
#include <Box2D/Collision/MassData.hpp>
#include <Box2D/Collision/RayCastOutput.hpp>

namespace box2d {

Expand Down
25 changes: 17 additions & 8 deletions Box2D/Testbed/Tests/EdgeShapes.hpp
View file
Expand Up @@ -78,19 +78,28 @@ class EdgeShapes : public Test

for (auto i = 0; i < 4; ++i)
{
m_polygons[i].SetFriction(0.3f);
m_polygons[i].SetDensity(RealNum{20} * KilogramPerSquareMeter);
m_polygons[i] = std::make_shared<PolygonShape>();
m_polygons[i]->SetFriction(0.3f);
m_polygons[i]->SetDensity(RealNum{20} * KilogramPerSquareMeter);
}

m_polygons[0].Set({Vec2(-0.5f, 0.0f) * Meter, Vec2(0.5f, 0.0f) * Meter, Vec2(0.0f, 1.5f) * Meter});
m_polygons[1].Set({Vec2(-0.1f, 0.0f) * Meter, Vec2(0.1f, 0.0f) * Meter, Vec2(0.0f, 1.5f) * Meter});
m_polygons[0]->Set({
Vec2(-0.5f, 0.0f) * Meter,
Vec2(0.5f, 0.0f) * Meter,
Vec2(0.0f, 1.5f) * Meter
});
m_polygons[1]->Set({
Vec2(-0.1f, 0.0f) * Meter,
Vec2(0.1f, 0.0f) * Meter,
Vec2(0.0f, 1.5f) * Meter
});

{
const auto w = 1.0f;
const auto b = w / (2.0f + Sqrt(2.0f));
const auto s = Sqrt(2.0f) * b;

m_polygons[2].Set({
m_polygons[2]->Set({
Vec2(0.5f * s, 0.0f) * Meter,
Vec2(0.5f * w, b) * Meter,
Vec2(0.5f * w, b + s) * Meter,
Expand All @@ -102,7 +111,7 @@ class EdgeShapes : public Test
});
}

m_polygons[3].SetAsBox(RealNum{0.5f} * Meter, RealNum{0.5f} * Meter);
m_polygons[3]->SetAsBox(RealNum{0.5f} * Meter, RealNum{0.5f} * Meter);

m_bodyIndex = 0;
memset(m_bodies, 0, sizeof(m_bodies));
Expand Down Expand Up @@ -135,7 +144,7 @@ class EdgeShapes : public Test

if (index < 4)
{
m_bodies[m_bodyIndex]->CreateFixture(std::make_shared<PolygonShape>(m_polygons[index]));
m_bodies[m_bodyIndex]->CreateFixture(m_polygons[index]);
}
else
{
Expand Down Expand Up @@ -221,7 +230,7 @@ class EdgeShapes : public Test

int m_bodyIndex;
Body* m_bodies[e_maxBodies];
PolygonShape m_polygons[4];
std::shared_ptr<PolygonShape> m_polygons[4];
std::shared_ptr<CircleShape> m_circle = std::make_shared<CircleShape>(RealNum{0.5f} * Meter);

RealNum m_angle;
Expand Down
2 changes: 2 additions & 0 deletions Box2D/Testbed/Tests/RayCast.hpp
View file
Expand Up @@ -179,6 +179,7 @@ class RayCast : public Test
m_circle->SetVertexRadius(RealNum{0.5f} * Meter);
m_circle->SetFriction(0.3f);
m_edge->SetFriction(0.3f);
m_edge->SetVertexRadius((RealNum(1) / RealNum(5)) * Meter);

// Ground body
const auto ground = m_world->CreateBody();
Expand All @@ -188,6 +189,7 @@ class RayCast : public Test
{
p = std::make_shared<PolygonShape>();
p->SetFriction(0.3f);
p->SetVertexRadius((RealNum(1) / RealNum(20)) * Meter);
}

m_polygons[0]->Set({Vec2(-0.5f, 0.0f) * Meter, Vec2(0.5f, 0.0f) * Meter, Vec2(0.0f, 1.5f) * Meter});
Expand Down

0 comments on commit a952a62

Please sign in to comment.