void buildMansardShape(const utymap::meshing::Polygon& polygon, ClipperLib::Path& offsetShape, std::size_t index)
{
std::reverse(offsetShape.begin(), offsetShape.end());
// build top
utymap::meshing::Polygon topShape(offsetShape.size(), 0);
std::vector<utymap::meshing::Vector2> topShapeVertices;
topShapeVertices.reserve(offsetShape.size());
for (const auto& p : offsetShape) {
topShapeVertices.push_back(utymap::meshing::Vector2(p.X / Scale, p.Y/ Scale));
}
topShape.addContour(topShapeVertices);
auto topOptions = utymap::meshing::MeshBuilder::Options{ 0, 0, colorNoiseFreq_, height_, getColorGradient(), minHeight_ };
builderContext_.meshBuilder.addPolygon(meshContext_.mesh, topShape, topOptions);
// build sides
auto sideOptions = utymap::meshing::MeshBuilder::Options { 0, 0, colorNoiseFreq_, 0, getColorGradient(), 0 };
double topHeight = minHeight_ + height_;
auto size = polygon.points.size();
for (std::size_t i = 0; i < size; i += 2) {
auto topIndex = i;
auto bottomIndex = (index + i) % size;
auto nextTopIndex = (i + 2) % size;
auto nextBottomIndex = (index + i + 2) % size;
auto v0 = utymap::meshing::Vector3(polygon.points[bottomIndex], minHeight_, polygon.points[bottomIndex + 1]);
auto v1 = utymap::meshing::Vector3(polygon.points[nextBottomIndex], minHeight_, polygon.points[nextBottomIndex + 1]);
auto v2 = utymap::meshing::Vector3(topShape.points[nextTopIndex], topHeight, topShape.points[nextTopIndex + 1]);
auto v3 = utymap::meshing::Vector3(topShape.points[topIndex], topHeight, topShape.points[topIndex + 1]);
builderContext_.meshBuilder.addTriangle(meshContext_.mesh, v0, v2, v3, sideOptions, false);
builderContext_.meshBuilder.addTriangle(meshContext_.mesh, v2, v0, v1, sideOptions, false);
}
}
void build(utymap::meshing::Polygon& polygon)
{
ClipperLib::ClipperOffset offset;
ClipperLib::Path path;
path.reserve(polygon.points.size() / 2);
auto lastPointIndex = polygon.points.size() - 2;
double min = std::numeric_limits<double>::max();
for (std::size_t i = 0; i < polygon.points.size(); i += 2) {
auto nextIndex = i == lastPointIndex ? 0 : i + 2;
utymap::meshing::Vector2 v1(polygon.points[i], polygon.points[i + 1]);
utymap::meshing::Vector2 v2(polygon.points[nextIndex], polygon.points[nextIndex + 1]);
min = std::min(min, utymap::meshing::Vector2::distance(v1, v2));
path.push_back(ClipperLib::IntPoint(static_cast<ClipperLib::cInt>(v1.x * Scale),
static_cast<ClipperLib::cInt>(v1.y * Scale)));
}
offset.AddPath(path, ClipperLib::JoinType::jtMiter, ClipperLib::EndType::etClosedPolygon);
ClipperLib::Paths solution;
// NOTE: use minimal side value as reference for offsetting.
offset.Execute(solution, -(min / 10) * Scale);
// NOTE: this is unexpected result for algorithm below, fallback to flat roof.
if (solution.size() != 1 || solution[0].size() != path.size()) {
return FlatRoofBuilder::build(polygon);
}
buildMansardShape(polygon, solution[0], findFirstIndex(solution[0][0], polygon));
}
void
ClipperPath_to_Slic3rMultiPoint(const ClipperLib::Path &input, T* output)
{
PROFILE_FUNC();
output->points.clear();
output->points.reserve(input.size());
for (ClipperLib::Path::const_iterator pit = input.begin(); pit != input.end(); ++pit)
output->points.push_back(Slic3r::Point( (*pit).X, (*pit).Y ));
}
static GeometryCoordinates fromClipperPath(const ClipperLib::Path& path) {
GeometryCoordinates result;
result.reserve(path.size() + 1);
result.reserve(path.size());
for (const auto& p : path) {
using Coordinate = GeometryCoordinates::coordinate_type;
assert(p.x >= std::numeric_limits<Coordinate>::min());
assert(p.x <= std::numeric_limits<Coordinate>::max());
assert(p.y >= std::numeric_limits<Coordinate>::min());
assert(p.y <= std::numeric_limits<Coordinate>::max());
result.emplace_back(Coordinate(p.x), Coordinate(p.y));
}
// Clipper does not repeat initial point, but our geometry model requires it.
if (!result.empty()) {
result.push_back(result.front());
}
return result;
}
static void polygon_Convert( const ClipperLib::Path &aPath,
SEGMENTS &aOutSegment,
float aBiuTo3DunitsScale )
{
aOutSegment.resize( aPath.size() );
for( unsigned i = 0; i < aPath.size(); i++ )
{
aOutSegment[i].m_Start = SFVEC2F( (float) aPath[i].X * aBiuTo3DunitsScale,
(float)-aPath[i].Y * aBiuTo3DunitsScale );
}
unsigned int i;
unsigned int j = aOutSegment.size () - 1;
for( i = 0; i < aOutSegment.size (); j = i++ )
{
// Calculate constants for each segment
aOutSegment[i].m_inv_JY_minus_IY = 1.0f / ( aOutSegment[j].m_Start.y -
aOutSegment[i].m_Start.y );
aOutSegment[i].m_JX_minus_IX = (aOutSegment[j].m_Start.x - aOutSegment[i].m_Start.x);
}
}
void Polygon2d_TestModule()
{
// "This structure contains a sequence of IntPoint vertices defining a
// single contour"
ClipperLib::Path aPath;
SEGMENTS aSegments;
aPath.resize( 4 );
aPath[0] = ClipperLib::IntPoint( -2, -2 );
aPath[1] = ClipperLib::IntPoint( 2, -2 );
aPath[2] = ClipperLib::IntPoint( 2, 2 );
aPath[3] = ClipperLib::IntPoint( -2, 2 );
// It must be an outter polygon
wxASSERT( ClipperLib::Orientation( aPath ) );
polygon_Convert( aPath, aSegments, 1.0f );
wxASSERT( aPath.size() == aSegments.size() );
wxASSERT( aSegments[0].m_Start == SFVEC2F( -2.0f, 2.0f ) );
wxASSERT( aSegments[1].m_Start == SFVEC2F( 2.0f, 2.0f ) );
wxASSERT( aSegments[2].m_Start == SFVEC2F( 2.0f, -2.0f ) );
wxASSERT( aSegments[3].m_Start == SFVEC2F( -2.0f, -2.0f ) );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( 0.0f, 0.0f ) ) );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( -1.9f, -1.9f ) ) );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( -1.9f, 1.9f ) ) );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( 1.9f, 1.9f ) ) );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( 1.9f, -1.9f ) ) );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( -2.1f, -2.0f ) ) == false );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( -2.1f, 2.0f ) ) == false );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( 2.1f, 2.0f ) ) == false );
wxASSERT( polygon_IsPointInside( aSegments, SFVEC2F( 2.1f, -2.0f ) ) == false );
}
inline void setCoordinates(T& t, const ClipperLib::Path& path) {
t.coordinates.reserve(path.size());
for (const auto& c : path) {
t.coordinates.push_back(GeoCoordinate(c.Y / Scale, c.X / Scale));
}
}
void BooleanTool::rebuildSegment(
ClipperLib::Path::size_type start_index,
ClipperLib::Path::size_type end_index,
bool sequence_increasing,
const ClipperLib::Path& polygon,
const PolyMap& polymap,
PathObject* object)
{
auto num_points = polygon.size();
object->getCoordinate(object->getCoordinateCount() - 1).setCurveStart(true);
if ((start_index + 1) % num_points == end_index)
{
// This could happen for a straight line or a very flat curve - take coords directly from original
rebuildTwoIndexSegment(start_index, end_index, sequence_increasing, polygon, polymap, object);
return;
}
// Get polygon point coordinates
const auto& start_point = polygon.at(start_index);
const auto& second_point = polygon.at((start_index + 1) % num_points);
const auto& second_last_point = polygon.at((end_index - 1) % num_points);
const auto& end_point = polygon.at(end_index);
// Try to find the middle coordinates in the same part
bool found = false;
PathCoordInfo second_info{ nullptr, nullptr };
PathCoordInfo second_last_info{ nullptr, nullptr };
for (auto second_it = polymap.find(second_point); second_it != polymap.end(); ++second_it)
{
for (auto second_last_it = polymap.find(second_last_point);
second_last_it != polymap.end() && second_last_it.key() == second_last_point;
++second_last_it)
{
if (second_it->first == second_last_it->first &&
second_it->second->index == second_last_it->second->index)
{
// Same part
found = true;
second_info = *second_it;
second_last_info = *second_last_it;
break;
}
}
if (found)
break;
}
if (!found)
{
// Need unambiguous path part information to find the original object with high probability
qDebug() << "BooleanTool::rebuildSegment: cannot identify original object!";
rebuildSegmentFromPathOnly(start_point, second_point, second_last_point, end_point, object);
return;
}
const PathPart* original_path = second_info.first;
// Try to find the outer coordinates in the same part
PathCoordInfo start_info{ nullptr, nullptr };
for (auto start_it = polymap.find(start_point);
start_it != polymap.end() && start_it.key() == start_point;
++start_it)
{
if (start_it->first == original_path)
{
start_info = *start_it;
break;
}
}
Q_ASSERT(!start_info.first || start_info.first == second_info.first);
PathCoordInfo end_info{ nullptr, nullptr };
for (auto end_it = polymap.find(end_point);
end_it != polymap.end() && end_it.key() == end_point;
++end_it)
{
if (end_it->first == original_path)
{
end_info = *end_it;
break;
}
}
Q_ASSERT(!end_info.first || end_info.first == second_info.first);
const PathObject* original = original_path->path;
auto edge_start = second_info.second->index;
if (edge_start == second_info.first->last_index)
edge_start = second_info.first->first_index;
// Find out start tangent
auto start_param = 0.0;
MapCoord start_coord = MapCoord(0.001 * start_point.X, 0.001 * start_point.Y);
MapCoord start_tangent;
MapCoord end_tangent;
MapCoord end_coord;
double start_error_sq, end_error_sq;
// Maximum difference in mm from reconstructed start and end coords to the
// intersection points returned by Clipper
const double error_bound = 0.4;
if (sequence_increasing)
{
if ( second_info.second->param == 0.0 ||
( start_info.first &&
start_info.second->param == 0.0 &&
( start_info.second->index == edge_start ||
(start_info.second->index == start_info.first->last_index && start_info.first->first_index == edge_start) ) ) )
{
// Take coordinates directly
start_tangent = original->getCoordinate(edge_start + 1);
end_tangent = original->getCoordinate(edge_start + 2);
start_error_sq = start_coord.distanceSquaredTo(original->getCoordinate(edge_start + 0));
if (start_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: start error too high in increasing direct case: " << sqrt(start_error_sq);
}
else
{
// Approximate coords
const PathCoord* prev_coord = second_info.second - 1;
auto dx = second_point.X - start_point.X;
auto dy = second_point.Y - start_point.Y;
auto point_dist = 0.001 * sqrt(dx*dx + dy*dy);
auto delta_start_param = (second_info.second->param - prev_coord->param) * point_dist / qMax(1e-7f, (second_info.second->clen - prev_coord->clen));
start_param = qBound(0.0, second_info.second->param - delta_start_param, 1.0);
MapCoordF unused, o2, o3, o4;
PathCoord::splitBezierCurve(MapCoordF(original->getCoordinate(edge_start + 0)), MapCoordF(original->getCoordinate(edge_start + 1)),
MapCoordF(original->getCoordinate(edge_start + 2)), MapCoordF(original->getCoordinate(edge_start + 3)),
start_param,
unused, unused, o2, o3, o4);
start_tangent = MapCoord(o3);
end_tangent = MapCoord(o4);
start_error_sq = start_coord.distanceSquaredTo(MapCoord(o2));
if (start_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: start error too high in increasing general case: " << sqrt(start_error_sq);
}
// Find better end point approximation and its tangent
if ( second_last_info.second->param == 0.0 ||
(end_info.first &&
end_info.second->param == 0.0 &&
( end_info.second->index == edge_start+3 ||
(end_info.second->index == end_info.first->first_index && end_info.first->last_index == edge_start+3) ) ) )
{
// Take coordinates directly
end_coord = original->getCoordinate(edge_start + 3);
auto test_x = end_point.X - end_coord.nativeX();
auto test_y = end_point.Y - end_coord.nativeY();
end_error_sq = 0.001 * sqrt(test_x*test_x + test_y*test_y);
if (end_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: end error too high in increasing direct case: " << sqrt(end_error_sq);
}
else
{
// Approximate coords
const PathCoord* next_coord = second_last_info.second + 1;
auto next_coord_param = next_coord->param;
if (next_coord_param == 0.0)
next_coord_param = 1.0;
auto dx = end_point.X - second_last_point.X;
auto dy = end_point.Y - second_last_point.Y;
auto point_dist = 0.001 * sqrt(dx*dx + dy*dy);
auto delta_end_param = (next_coord_param - second_last_info.second->param) * point_dist / qMax(1e-7f, (next_coord->clen - second_last_info.second->clen));
auto end_param = (second_last_info.second->param + delta_end_param - start_param) / (1.0 - start_param);
MapCoordF o0, o1, o2, unused;
PathCoord::splitBezierCurve(MapCoordF(start_coord), MapCoordF(start_tangent),
MapCoordF(end_tangent), MapCoordF(original->getCoordinate(edge_start + 3)),
end_param,
o0, o1, o2, unused, unused);
start_tangent = MapCoord(o0);
end_tangent = MapCoord(o1);
end_coord = MapCoord(o2);
auto test_x = end_point.X - end_coord.nativeX();
auto test_y = end_point.Y - end_coord.nativeY();
end_error_sq = 0.001 * sqrt(test_x*test_x + test_y*test_y);
if (end_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: end error too high in increasing general case: " << sqrt(end_error_sq);
}
}
else // if (!sequence_increasing)
{
if ( second_info.second->param == 0.0 ||
( start_info.first &&
start_info.second->param == 0.0 &&
( start_info.second->index == edge_start+3 ||
(start_info.second->index == start_info.first->first_index && start_info.first->last_index == edge_start+3) ) ) )
{
// Take coordinates directly
start_tangent = original->getCoordinate(edge_start + 2);
end_tangent = original->getCoordinate(edge_start + 1);
start_error_sq = start_coord.distanceSquaredTo(original->getCoordinate(edge_start + 3));
if (start_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: start error too high in decreasing direct case: " << sqrt(start_error_sq);
}
else
{
// Approximate coords
const PathCoord* next_coord = second_info.second + 1;
auto next_coord_param = next_coord->param;
if (next_coord_param == 0.0)
next_coord_param = 1.0;
auto dx = second_point.X - start_point.X;
auto dy = second_point.Y - start_point.Y;
auto point_dist = 0.001 * sqrt(dx*dx + dy*dy);
auto delta_start_param = (next_coord_param - second_info.second->param) * point_dist / qMax(1e-7f, (next_coord->clen - second_info.second->clen));
start_param = qBound(0.0, 1.0 - second_info.second->param + delta_start_param, 1.0);
MapCoordF unused, o2, o3, o4;
PathCoord::splitBezierCurve(MapCoordF(original->getCoordinate(edge_start + 3)), MapCoordF(original->getCoordinate(edge_start + 2)),
MapCoordF(original->getCoordinate(edge_start + 1)), MapCoordF(original->getCoordinate(edge_start + 0)),
start_param,
unused, unused, o2, o3, o4);
start_tangent = MapCoord(o3);
end_tangent = MapCoord(o4);
start_error_sq = start_coord.distanceSquaredTo(MapCoord(o2));
if (start_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: start error too high in decreasing general case: " << sqrt(start_error_sq);
}
// Find better end point approximation and its tangent
if ( second_last_info.second->param == 0.0 ||
( end_info.first &&
end_info.second->param == 0.0 &&
( end_info.second->index == edge_start ||
(end_info.second->index == end_info.first->last_index && end_info.first->first_index == edge_start) ) ) )
{
// Take coordinates directly
end_coord = original->getCoordinate(edge_start + 0);
auto test_x = end_point.X - end_coord.nativeX();
auto test_y = end_point.Y - end_coord.nativeY();
end_error_sq = 0.001 * sqrt(test_x*test_x + test_y*test_y);
if (end_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: end error too high in decreasing direct case: " << sqrt(end_error_sq);
}
else
{
// Approximate coords
const PathCoord* prev_coord = second_last_info.second - 1;
auto dx = end_point.X - second_last_point.X;
auto dy = end_point.Y - second_last_point.Y;
auto point_dist = 0.001 * sqrt(dx*dx + dy*dy);
auto delta_end_param = (second_last_info.second->param - prev_coord->param) * point_dist / qMax(1e-7f, (second_last_info.second->clen - prev_coord->clen));
auto end_param = (1.0 - second_last_info.second->param + delta_end_param) / (1 - start_param);
MapCoordF o0, o1, o2, unused;
PathCoord::splitBezierCurve(MapCoordF(start_coord), MapCoordF(start_tangent),
MapCoordF(end_tangent), MapCoordF(original->getCoordinate(edge_start + 0)),
end_param,
o0, o1, o2, unused, unused);
start_tangent = MapCoord(o0);
end_tangent = MapCoord(o1);
end_coord = MapCoord(o2);
auto test_x = end_point.X - end_coord.nativeX();
auto test_y = end_point.Y - end_coord.nativeY();
end_error_sq = 0.001 * sqrt(test_x*test_x + test_y*test_y);
if (end_error_sq > error_bound)
qDebug() << "BooleanTool::rebuildSegment: end error too high in decreasing general case: " << sqrt(end_error_sq);
}
}
if (start_error_sq <= error_bound && end_error_sq <= error_bound)
{
// Rebuild bezier curve using information from original curve
object->addCoordinate(start_tangent);
object->addCoordinate(end_tangent);
object->addCoordinate(resetCoordinate(end_coord));
}
else
{
// Rebuild bezier curve approximately using tangents derived from result polygon
rebuildSegmentFromPathOnly(start_point, second_point, second_last_point, end_point, object);
}
}
void BooleanTool::polygonToPathPart(const ClipperLib::Path& polygon, const PolyMap& polymap, PathObject* object)
{
auto num_points = polygon.size();
if (num_points < 3)
return;
// Index of first used point in polygon
auto part_start_index = 0u;
auto cur_info = PathCoordInfo{ nullptr, nullptr };
// Check if we can find either an unknown intersection point
// or a path coord with parameter 0.
// This gives a starting point to search for curves to rebuild
// (because we cannot start in the middle of a curve)
for (; part_start_index < num_points; ++part_start_index)
{
auto current_point = polygon.at(part_start_index);
if (!polymap.contains(current_point))
break;
if (polymap.value(current_point).second->param == 0.0)
{
cur_info = polymap.value(current_point);
break;
}
}
if (part_start_index == num_points)
{
// Did not find a valid starting point. Return the part as a polygon.
for (auto i = 0u; i < num_points; ++i)
object->addCoordinate(MapCoord(0.001 * polygon.at(i).X, 0.001 * polygon.at(i).Y), (i == 0));
object->parts().back().setClosed(true, true);
return;
}
// Add the first point to the object
rebuildCoordinate(part_start_index, polygon, polymap, object, true);
// Index of first segment point in polygon
auto segment_start_index = part_start_index;
bool have_sequence = false;
bool sequence_increasing = false;
bool stop_before = false;
// Advance along the boundary and rebuild the curve for every sequence
// of path coord pointers with the same path and index.
auto i = part_start_index;
do
{
++i;
if (i >= num_points)
i = 0;
PathCoordInfo new_info{ nullptr, nullptr };
auto new_point = polygon.at(i);
if (polymap.contains(new_point))
new_info = polymap.value(new_point);
if (cur_info.first && cur_info.first == new_info.first)
{
// Same original part
auto cur_coord_index = cur_info.second->index;
MapCoord& cur_coord = cur_info.first->path->getCoordinate(cur_coord_index);
auto new_coord_index = new_info.second->index;
MapCoord& new_coord = new_info.first->path->getCoordinate(new_coord_index);
auto cur_coord_index_adjusted = cur_coord_index;
if (cur_coord_index_adjusted == new_info.first->first_index)
cur_coord_index_adjusted = new_info.first->last_index;
auto new_coord_index_adjusted = new_coord_index;
if (new_coord_index_adjusted == new_info.first->first_index)
new_coord_index_adjusted = new_info.first->last_index;
if (cur_coord_index == new_coord_index)
{
// Somewhere on a curve
bool param_increasing = new_info.second->param > cur_info.second->param;
if (!have_sequence)
{
have_sequence = true;
sequence_increasing = param_increasing;
}
else if (have_sequence && sequence_increasing != param_increasing)
{
stop_before = true;
}
}
else if (new_info.second->param == 0.0 &&
( (cur_coord.isCurveStart() && new_coord_index_adjusted == cur_coord_index + 3) ||
(!cur_coord.isCurveStart() && new_coord_index_adjusted == cur_coord_index + 1) ) )
{
// Original curve is from cur_coord_index to new_coord_index_adjusted.
if (!have_sequence)
{
have_sequence = true;
sequence_increasing = true;
}
else
{
stop_before = !sequence_increasing;
}
}
else if (cur_info.second->param == 0.0 &&
( (new_coord.isCurveStart() && new_coord_index + 3 == cur_coord_index_adjusted) ||
(!new_coord.isCurveStart() && new_coord_index + 1 == cur_coord_index_adjusted) ) )
{
// Original curve is from new_coord_index to cur_coord_index_adjusted.
if (!have_sequence)
{
have_sequence = true;
sequence_increasing = false;
}
else
{
stop_before = sequence_increasing;
}
}
else if ((segment_start_index + 1) % num_points != i)
{
// Not immediately after segment_start_index
stop_before = true;
}
}
if (i == part_start_index ||
stop_before ||
(new_info.second && new_info.second->param == 0.0) ||
(cur_info.first && (cur_info.first != new_info.first || cur_info.second->index != new_info.second->index) && i != (segment_start_index + 1) % num_points) ||
!new_info.first)
{
if (stop_before)
{
if (i == 0)
i = num_points - 1;
else
--i;
}
if (have_sequence)
// A sequence of at least two points belonging to the same curve
rebuildSegment(segment_start_index, i, sequence_increasing, polygon, polymap, object);
else
// A single straight edge
rebuildCoordinate(i, polygon, polymap, object);
if (stop_before)
{
++i;
if (i >= num_points)
i = 0;
rebuildCoordinate(i, polygon, polymap, object);
stop_before = false;
}
segment_start_index = i;
have_sequence = false;
}
cur_info = new_info;
}
while (i != part_start_index);
object->parts().back().connectEnds();
}
void World::update(cv::Mat &homography)
{
this->m_world->Step(dt, 10, 10);
//check contacts
std::vector<MyContact>::iterator pos;
std::map<b2Body*, ClipperLib::Paths*> newBodyMap;
std::vector<b2Body*> removeBarrierList;
for(pos = this->m_contactListener->m_contacts.begin();
pos != this->m_contactListener->m_contacts.end();
++pos)
{
MyContact contact = *pos;
if ((contact.fixtureA == this->m_ballFixture || contact.fixtureB == this->m_ballFixture)
&& (contact.fixtureA->GetBody() != m_groundBody && contact.fixtureB->GetBody() != m_groundBody)
&& (contact.fixtureA->GetBody() != m_paddlesBody && contact.fixtureB->GetBody() != m_paddlesBody))
{
b2Fixture* objectFixture = contact.fixtureA == this->m_ballFixture ? contact.fixtureB : contact.fixtureA;
b2Body *objectBody = objectFixture->GetBody();
if (objectFixture->GetType() == b2Shape::e_edge)
{
cv::Point2f hitPoint = CVUtils::transformPoint(cv::Point2f(contact.contactPoint->x * PTM_RATIO, contact.contactPoint->y * PTM_RATIO), homography);
this->notifyBallHitObservers(hitPoint.x, hitPoint.y);
// change the shape of the fixture
// only go into processing if this body was not processed yet (possible ball hit two fixture of same body)
if (newBodyMap.find(objectBody) == newBodyMap.end())
{
ClipperLib::Paths* bodyPolygons = (ClipperLib::Paths*)objectBody->GetUserData();
b2Vec2* impactVelocity = contact.fixtureA == m_ballFixture ? contact.impactVelocityA : contact.impactVelocityB;
float ballAngle = atan2(impactVelocity->y, impactVelocity->x); // get the angle (in radians) the ball is moving to
float ballPower = impactVelocity->Length() * 0.5; // get the "power" of the ball movement vector
float openingStepInRadians = 10 * CV_PI / 180; // calculate the opening in radians
// create the clipping object/shape - a wedge from ball's center with 30 degree opening over ball direction (angle)
ClipperLib::Path clip;
clip.push_back(ClipperLib::IntPoint(contact.contactPoint->x * PTM_RATIO, contact.contactPoint->y * PTM_RATIO));
for(int step = 9; step > -10; step--)
{
float dx = cos(ballAngle + step * openingStepInRadians) * ballPower;
float dy = sin(ballAngle + step * openingStepInRadians) * ballPower;
clip.push_back(ClipperLib::IntPoint(contact.contactPoint->x * PTM_RATIO + dx, contact.contactPoint->y * PTM_RATIO + dy));
}
ClipperLib::Clipper clipper;
clipper.AddPaths((*bodyPolygons), ClipperLib::ptSubject, true);
clipper.AddPath(clip, ClipperLib::ptClip, true);
// the difference is the new polygon formed by the clipping (collision)
ClipperLib::Paths* newPolygons = new ClipperLib::Paths();
clipper.Execute(ClipperLib::ctDifference, (*newPolygons), ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
// Save the new polygons of this body
objectBody->SetUserData(newPolygons);
newBodyMap[objectBody] = newPolygons;
// now, find the intersection regions - these should be inpainted to the scene
ClipperLib::Paths destroyedParts;
clipper.Execute(ClipperLib::ctIntersection, destroyedParts, ClipperLib::pftEvenOdd, ClipperLib::pftEvenOdd);
// paint the required areas to be coppied
for (size_t i = 0; i < destroyedParts.size(); i++)
{
cv::Point* points = new cv::Point[destroyedParts[i].size()];
for (size_t j = 0; j < destroyedParts[i].size(); j++)
{
points[j].x = (int)destroyedParts[i][j].X;
points[j].y = (int)destroyedParts[i][j].Y;
}
m_destroyedPolygons.push_back(points);
m_destroyedPolygonsPointCount.push_back((int)destroyedParts[i].size());
}
}
}
else if (objectFixture->GetType() == b2Shape::e_circle)
{
// this is a barrier - add it to the remove list
removeBarrierList.push_back(objectBody);
}
}
}
std::map<b2Body*, ClipperLib::Paths*>::iterator iter;
for(iter = newBodyMap.begin(); iter != newBodyMap.end(); iter++)
{
b2Body* objectBody = iter->first;
ClipperLib::Paths* newPolygons = iter->second;
// remove all the current fixtures from this body
for (b2Fixture* f = objectBody->GetFixtureList(); f; )
{
b2Fixture* fixtureToDestroy = f;
f = f->GetNext();
objectBody->DestroyFixture( fixtureToDestroy );
}
if(newPolygons->size() == 0)
{
// there is no more pieces of the object left so remove it from list and world
m_objectBodies.erase(std::find(m_objectBodies.begin(), m_objectBodies.end(), objectBody));
m_world->DestroyBody(objectBody); // TODO: better physics world cleanup
}
else
{
for (size_t i = 0; i < newPolygons->size(); i++)
{
b2EdgeShape objectEdgeShape;
b2FixtureDef objectShapeDef;
objectShapeDef.shape = &objectEdgeShape;
ClipperLib::Path polygon = newPolygons->at(i);
size_t j;
for (j = 0; j < polygon.size() - 1; j++)
{
objectEdgeShape.Set(b2Vec2(polygon[j].X / PTM_RATIO, polygon[j].Y / PTM_RATIO), b2Vec2(polygon[j+1].X / PTM_RATIO, polygon[j+1].Y / PTM_RATIO));
objectBody->CreateFixture(&objectShapeDef);
}
objectEdgeShape.Set(b2Vec2(polygon[j].X / PTM_RATIO, polygon[j].Y / PTM_RATIO), b2Vec2(polygon[0].X / PTM_RATIO, polygon[0].Y / PTM_RATIO));
objectBody->CreateFixture(&objectShapeDef);
}
}
}
for (size_t i = 0; i < removeBarrierList.size(); i++){
cv::Point2f* p = (cv::Point2f*)removeBarrierList[i]->GetUserData();
std::vector<cv::Point2f*>::iterator position = std::find(m_guardLocations.begin(), m_guardLocations.end(), p);
if (position != m_guardLocations.end()){ // == vector.end() means the element was not found
m_guardLocations.erase(position);
}
removeBarrierList[i]->GetWorld()->DestroyBody(removeBarrierList[i]);
}
}
