/*!
*/
bool
Segment2D::existIntersection( const Line2D & l ) const
{
double a0 = l.a() * origin().x + l.b() * origin().y + l.c();
double a1 = l.a() * terminal().x + l.b() * terminal().y + l.c();
return a0 * a1 <= 0.0;
}
bool Stage::hasVision(Line2D *visionLine) {
for (int z = 0; z < numInnerWallLines_; z++) {
Line2D* wallLine = innerWallLines_[z];
if (wallLine->intersects(visionLine)) {
return false;
}
}
return true;
}
bool intersect2d::aabb_line(const AABB_2D &aabb, const Line2D &line) {
glm::vec2 l_min = line.min();
glm::vec2 l_max = line.max();
l_min = glm::max(l_min, aabb.min);
l_max = glm::min(l_max, aabb.max);
return !( (l_min.x > l_max.x) || (l_min.y > l_max.y));
}
/* ClassifyPathToCell
------------------------------------------------------------------------------------------
Classifies a Path in relationship to this cell. A path is represented by a 2D line
where Point A is the start of the path and Point B is the desired position.
If the path exits this cell on a side which is linked to another cell, that cell index
is returned in the NextCell parameter and SideHit contains the side number of the wall
exited through.
If the path collides with a side of the cell which has no link (a solid edge),
SideHit contains the side number (0-2) of the colliding wall.
In either case PointOfIntersection will contain the point where the path intersected
with the wall of the cell if it is provided by the caller.
------------------------------------------------------------------------------------------
*/
NavigationCell::PATH_RESULT NavigationCell::ClassifyPathToCell(const Line2D& MotionPath, NavigationCell** pNextCell, CELL_SIDE& Side, vector2* pPointOfIntersection)const
{
int InteriorCount = 0;
// Check our MotionPath against each of the three cell walls
for (int i=0; i<3; ++i)
{
// Classify the MotionPath endpoints as being either ON_LINE,
// or to its LEFT_SIDE or RIGHT_SIDE.
// Since our triangle vertices are in clockwise order,
// we know that points to the right of each line are inside the cell.
// Points to the left are outside.
// We do this test using the ClassifyPoint function of Line2D
// If the destination endpoint of the MotionPath
// is Not on the right side of this wall...
if (m_Side[i].ClassifyPoint(MotionPath.EndPointB()) != Line2D::RIGHT_SIDE)
{
// ..and the starting endpoint of the MotionPath
// is Not on the left side of this wall...
if (m_Side[i].ClassifyPoint(MotionPath.EndPointA()) != Line2D::LEFT_SIDE)
{
// Check to see if we intersect the wall
// using the Intersection function of Line2D
Line2D::LINE_CLASSIFICATION IntersectResult = MotionPath.Intersection(m_Side[i], pPointOfIntersection);
if (IntersectResult == Line2D::SEGMENTS_INTERSECT || IntersectResult == Line2D::A_BISECTS_B)
{
// record the link to the next adjacent cell
// (or NULL if no attachement exists)
// and the enumerated ID of the side we hit.
*pNextCell = m_Link[i];
Side = (CELL_SIDE)i;
return (EXITING_CELL);
}
}
}
else
{
// The destination endpoint of the MotionPath is on the right side.
// Increment our InteriorCount so we'll know how many walls we were
// to the right of.
InteriorCount++;
}
}
// An InteriorCount of 3 means the destination endpoint of the MotionPath
// was on the right side of all walls in the cell.
// That means it is located within this triangle, and this is our ending cell.
if (InteriorCount == 3)
{
return (ENDING_CELL);
}
// We only reach here is if the MotionPath does not intersect the cell at all.
return (NO_RELATIONSHIP);
}
void SC_Triangulation::EdgeBlockCleanup(const SC_PolyStore& nodeBlocks)
{
// make list of OK edges
int nEdges = edgeList.Size();
SC_BoolArray triOK(GetNTriangles());
SC_PointArray stBlock;
SC_PointArray endBlock;
Line2D nodeLine;
Line2D resultLine;
bool allOK;
// repeat for outside edges until none found
while (true)
{
// reset to OK
triOK.FillToSize(true);
allOK = true;
for (int i = 0; i < nEdges; i++)
{
TriangleEdge& currEdge = edgeList[i];
// skip edges that are already deleted or are not outside
if ((currEdge.tri1 < 0) || (currEdge.tri2 >= 0))
continue;
// set node line endpoints
nodeLine.stPt = nodePoints[currEdge.stNode];
nodeLine.endPt = nodePoints[currEdge.endNode];
double fullDist = nodeLine.Length();
double remDist = fullDist;
nodeBlocks.GetPolyPoints(currEdge.stNode, stBlock);
if (stBlock.IntersectLine(nodeLine, resultLine, false, false))
remDist -= resultLine.Length();
nodeBlocks.GetPolyPoints(currEdge.endNode, endBlock);
if (endBlock.IntersectLine(nodeLine, resultLine, false, false))
remDist -= resultLine.Length();
if ((remDist / fullDist) > 0.01)
{
triOK[currEdge.tri1] = false;
allOK = false;
}
}
if (allOK)
return;
DoCleanup(triOK);
}
}
bool Box2D::intersect(const Line2D& line) const {
const double x1 = -(line.b()*topLeft_.v() + line.c())/line.a();
const double x2 = -(line.b()*bottomRight_.v() + line.c())/line.a();
if(x1 < topLeft_.u() && x2 < topLeft_.u()) return false;
if(x1 > bottomRight_.u() && x2 > bottomRight_.u()) return false;
const double y1 = -(line.a()*topLeft_.u() + line.c())/line.b();
const double y2 = -(line.a()*bottomRight_.u() + line.c())/line.b();
if(y1 < topLeft_.v() && y2 < topLeft_.v()) return false;
if(y1 > bottomRight_.v() && y2 > bottomRight_.v()) return false;
return true;
}
/*! First all intersections are calculated. Then duplicates are removed.
If only points on the line are required, all points that are outside
the lnX[A,B] are removed. If bParallel is true, parallel segmets are
checked for overlap and overlap points are included in the solution.
\retval VecVertex container of intersection points.
*/
QVector<Point2D> Polygon2D::GetIntersection(
const Line2D& lnX, //!< line that intersects the polygon
bool bOnLineOnly, //!< if true, only points between A and B inclusive are counted.
bool bParallel //!< check parallel lines for overlap
) const
{
// results are stored here
QVector<Point2D> vecInt;
// for each segment
for(unsigned int i=0; i<GetCount(); i++) {
Line2D ln = GetLine(i);
Q_ASSERT(ln.IsValid());
Point2D ptInt;
// Check overlaps, if they are wanted
if(bParallel && ln.IsParallel(lnX)) {
Line2D lnO = ln.GetOverlap(lnX);
if(lnO.IsValid()) {
vecInt.push_back(lnO.GetA());
vecInt.push_back(lnO.GetB());
}
}
// check intersections
if(ln.GetIntersection(lnX, ptInt) & Line2D::intFirst){
vecInt.push_back(ptInt);
}
}
// Sort the points in the vector. Same point may appear several
// times.
Point2DSortLineLess pred(lnX);
std::sort(vecInt.begin(),vecInt.end(),pred);
// remove duplicates, if any
QVector<Point2D>::iterator f = std::unique(vecInt.begin(),vecInt.end());
vecInt.erase(f,vecInt.end());
// remove all points that are not inside the line
if(bOnLineOnly) {
QVector<Point2D>::iterator i=vecInt.begin();
while(i!=vecInt.end()) {
if(!(lnX.GetPosition(*i) & Line2D::posInsideEnd))
i = vecInt.erase(i);
else
i++;
}
}
return vecInt;
}
/* ProjectPathOnCellWall
------------------------------------------------------------------------------------------
ProjectPathOnCellWall projects a path intersecting the wall with the wall itself. This
can be used to convert a path colliding with a cell wall to a resulting path moving
along the wall. The input parameter MotionPath MUST contain a starting point (EndPointA)
which is the point of intersection with the path and cell wall number [SideNumber]
and an ending point (EndPointB) which resides outside of the cell.
------------------------------------------------------------------------------------------
*/
void NavigationCell::ProjectPathOnCellWall(CELL_SIDE SideNumber, Line2D& MotionPath)const
{
// compute the normalized vector of the cell wall in question
vector2 WallNormal = m_Side[SideNumber].EndPointB() - m_Side[SideNumber].EndPointA();
WallNormal.normalize();
// determine the vector of our current movement
vector2 MotionVector = MotionPath.EndPointB() - MotionPath.EndPointA();
// compute dot product of our MotionVector and the normalized cell wall
// this gives us the magnatude of our motion along the wall
float DotResult = DotProduct(MotionVector,WallNormal);
// our projected vector is then the normalized wall vector times our new found magnatude
MotionVector = (DotResult * WallNormal);
// redirect our motion path along the new reflected direction
MotionPath.SetEndPointB(MotionPath.EndPointA() + MotionVector);
//
// Make sure starting point of motion path is within the cell
//
vector2 NewPoint = MotionPath.EndPointA();
ForcePointToCellCollumn(NewPoint);
MotionPath.SetEndPointA(NewPoint);
//
// Make sure destination point does not intersect this wall again
//
NewPoint = MotionPath.EndPointB();
ForcePointToWallInterior(SideNumber, NewPoint);
MotionPath.SetEndPointB(NewPoint);
}
/*! Distances from the line and vertices are considered. In the case of
line, perpendicuar distance is used in the case of vertex, radial
distance is used.
\retval qreal minimal distance to the polygon border.
*/
qreal Polygon2D::GetDistance(
const Point2D& pt //!< distance to this point is calculated.
) const
{
qreal dD = std::numeric_limits<qreal>::max();
for(unsigned int i=0; i<GetCount(); i++) {
dD = GetMin(dD, pt.GetDistance(GetVertex(i)));
Line2D ln = GetLine(i);
Point2D ptN = ln.GetNearestPoint(pt);
if(ln.GetPosition(ptN) & Line2D::posInside)
dD = GetMin(dD, ptN.GetDistance(pt));
}
return dD;
}
Point2D
Line2D::intersectLine(Line2D &l) {
double x,y,h;
Point2D p;
h = this->v(0)*l.v(1) - this->v(1)*l.v(0); /*x1*y2 - y1*x2*/
/*Are parallel*/
if(h==0)
return Point2D(0.0,0.0,0.0);
else {
x = (this->v(1)*l.v(2) - l.v(1)*this->v(2))/h; /*y1*z2 - z1*y2*/
y = (this->v(2)*l.v(0) - this->v(0)*l.v(2))/h; /*z1*x2 - x1*z2*/
return Point2D(x,y,1.0);
}
}
/* --------------------------------------------------------------------- */
bool
Circle2D::HasIntersection(Line2D line )
{
double dist = line.dist(this->center());
if (dist > this->radius()) return false;
else return true;
}
bool Line2D::IntersectAndContain (const Line2D& rclLine, Vector2D &rclV) const
{
bool rc = Intersect (rclLine, rclV);
if (rc)
rc = Contains (rclV) && rclLine.Contains (rclV);
return rc;
}
bool Stage::touchedZone(Ship *oldShip, Ship *ship, Zone *zone) {
if (inZone(ship, zone)) {
return true;
}
Line2D *line = new Line2D(oldShip->x, oldShip->y, ship->x, ship->y);
Line2D **zoneLines = zone->getLines();
for (int x = 0; x < 4; x++) {
Line2D *zoneLine = zoneLines[x];
if (zoneLine->intersects(line)) {
delete line;
return true;
}
}
delete line;
return false;
}
bool FeatureConnector::storeBinaryDataLine(FeatureCoverage *fcov, const QString& baseName) {
std::ofstream output_file;
QFileInfo inf(baseName);
QString dir = context()->workingCatalog()->location().toLocalFile();
QString filename = dir + "/" + inf.baseName() + ".mps#";
output_file.open(filename.toLatin1(),ios_base::out | ios_base::binary | ios_base::trunc);
if ( !output_file.is_open())
return ERROR1(ERR_COULD_NOT_OPEN_WRITING_1,filename);
char header[128];
memset(header, 0, 128);
output_file.write(header,128);
IFeatureCoverage cov;
cov.set(fcov);
FeatureIterator iter(cov);
quint32 raw = 1;
for_each(iter, iter.end(), [&](SPFeatureI feature){
const Geometry& geom = feature->geometry();
for(int i=0; i < feature->trackSize(); ++i) {
if ( geom.ilwisType() == itLINE) {
Line2D<Coordinate2d> line = geom.toType<Line2D<Coordinate2d>>();
const Coordinate2d& crdmin = geom.envelope().min_corner();
const Coordinate2d& crdmax = geom.envelope().max_corner();
writeCoord(output_file, crdmin);
writeCoord(output_file, crdmax);
int noOfCoordsBytes = line.size() * 16;
output_file.write((char *)&noOfCoordsBytes, 4);
for(const Coordinate2d& crd: line) {
writeCoord(output_file, crd);
}
long deleted=1;
output_file.write((char *)&deleted, 4);
output_file.write((char *)&raw, 4);
++raw;
}
}
});
output_file.close();
return true;
}
void Slicer::removeRepeated()
{
for(int k = 0; k < slice_byLine->size(); k++)
{
Line2D l = slice_byLine->at(k);
for(int i = k + 1; i < slice_byLine->size(); )
{
if(l.isEqual2(slice_byLine->at(i)))
{
slice_byLine->removeAt(i);
}
else
{
i++;
}
}
}
}
template<class T> Point2<T> Line2D<T>::intersectPoint(const Line2D<T> &l2) const
{
T d = (a.X - b.X)*(l2.getA().Y - l2.getB().Y) - (a.Y - b.Y)*(l2.getA().X - l2.getB().X);
T intersectX = ((l2.getA().X - l2.getB().X) * (a.X*b.Y - a.Y*b.X) - (a.X - b.X) * (l2.getA().X*l2.getB().Y - l2.getA().Y*l2.getB().X)) / d;
T intersectY = ((l2.getA().Y - l2.getB().Y) * (a.X*b.Y - a.Y*b.X) - (a.Y - b.Y) * (l2.getA().X*l2.getB().Y - l2.getA().Y*l2.getB().X)) / d;
return Point2<T>(intersectX, intersectY);
}
/*! For the oCCW (positive) polygon orientation a positive dOff will
inflate the polygon. For the oCW (negative) polygon orientation
a positive dOff will shrink the polygon.
*/
void Polygon2D::Offset(
qreal dOff //!< Offset value.
)
{
VecVertex vecNew;
vecNew.reserve(GetCount());
// Move each point
for(unsigned int i=0; i<GetCount(); i++) {
Line2D lnA = GetLine(i==0 ? GetCount()-1 : i-1);
Line2D lnB = GetLine(i);
// Offset both lines
lnA.Offset(-dOff);
lnB.Offset(-dOff);
// Intersection gives the new point;
Point2D ptI;
unsigned int uStatus = lnA.GetIntersection(lnB, ptI);
if(uStatus & Line2D::intExist) {
vecNew.push_back(ptI);
}
// lnA and lnB are parallel
else {
vecNew.push_back(lnA.GetB());
}
}
Clear();
m_vecV = vecNew;
}
bool FeatureConnector::loadBinarySegments(FeatureCoverage *fcoverage) {
BinaryIlwis3Table mpsTable;
if ( !mpsTable.load(_odf)) {
return ERROR1(ERR_COULD_NOT_OPEN_READING_1,_odf->fileinfo().fileName()) ;
}
int colCoords = mpsTable.index("Coords");
int colItemId = mpsTable.index("SegmentValue");
bool isNumeric = _odf->value("BaseMap","Range") != sUNDEF;
ITable tbl = fcoverage->attributeTable();
// if ( isNumeric) // in other case nr of record already has been set as it is based on a real table
// tbl->setRows(mpsTable.rows());
double value;
for(quint32 i= 0; i < mpsTable.rows(); ++i) {
std::vector<Coordinate > coords;
mpsTable.get(i,colCoords,coords);
Line2D<Coordinate2d > line;
line.resize(coords.size());
std::copy(coords.begin(), coords.end(), line.begin());
mpsTable.get(i, colItemId,value);
if ( isNumeric) {
tbl->cell(COVERAGEKEYCOLUMN, i, QVariant(i));
tbl->cell(FEATUREVALUECOLUMN, i, QVariant(value));
SPFeatureI feature = fcoverage->newFeature({line});
tbl->cell(FEATUREIDCOLUMN, i, QVariant(feature->featureid()));
} else {
quint32 itemId = value;
tbl->cell(COVERAGEKEYCOLUMN, i, QVariant(itemId));
SPFeatureI feature = fcoverage->newFeature({line});
tbl->cell(FEATUREIDCOLUMN, i, QVariant(feature->featureid()));
}
}
return true;
}
bool
Line2D::parallelTo(Line2D &l, double threshold) {
double diff;
diff = this->getAngle() - l.getAngle();
/*Normalize*/
while(diff < -GEOMETRY_PI_2)
diff += GEOMETRY_PI;
while(diff > GEOMETRY_PI_2)
diff -= GEOMETRY_PI;
return fabs(diff) < threshold;
}
bool BoundBox2D::operator|| (const Line2D &rclLine) const
{
Line2D clThisLine;
Vector2D clVct;
// first line
clThisLine.clV1.fX = fMinX;
clThisLine.clV1.fY = fMinY;
clThisLine.clV2.fX = fMaxX;
clThisLine.clV2.fY = fMinY;
if (clThisLine.IntersectAndContain (rclLine, clVct))
return true;
// second line
clThisLine.clV1 = clThisLine.clV2;
clThisLine.clV2.fX = fMaxX;
clThisLine.clV2.fY = fMaxY;
if (clThisLine.IntersectAndContain (rclLine, clVct))
return true;
// third line
clThisLine.clV1 = clThisLine.clV2;
clThisLine.clV2.fX = fMinX;
clThisLine.clV2.fY = fMaxY;
if (clThisLine.IntersectAndContain (rclLine, clVct))
return true;
// fourth line
clThisLine.clV1 = clThisLine.clV2;
clThisLine.clV2.fX = fMinX;
clThisLine.clV2.fY = fMinY;
if (clThisLine.IntersectAndContain (rclLine, clVct))
return true;
return false;
}
void ConvexPolygon2D::ClipToLine(const Line2D &line) {
if (GetPointCount() == 0) return;
std::vector<D3DXVECTOR2> new_points;
D3DXVECTOR2 p0 = points_[points_.size()-1];
bool p0_inside = line.Distance(p0) < 0;
std::vector<D3DXVECTOR2>::const_iterator it;
for (it = points_.begin(); it != points_.end(); ++it) {
const D3DXVECTOR2 &p1 = *it;
bool p1_inside = line.Distance(p1) < 0;
if (p0_inside && p1_inside) {
new_points.push_back(p1);
} else if (p0_inside && !p1_inside) {
new_points.push_back(line.Intersection(Line2D(p0, p1)));
} else if (!p0_inside && p1_inside) {
new_points.push_back(line.Intersection(Line2D(p0, p1)));
new_points.push_back(p1);
} else {
// do nothing
}
p0 = p1;
p0_inside = p1_inside;
}
points_.swap(new_points);
}
bool Polygon2D::AnyIntersection(
const Polygon2D& pg,
bool bBorder,
qreal dTol //!< Tolerance settings.
) const
{
for(unsigned int i=0; i<GetCount(); i++) {
Line2D lnA = GetLine(i);
for(unsigned int j=0; j<pg.GetCount(); j++) {
Line2D lnB = pg.GetLine(j);
// Check the intersection
Point2D X;
int iFlag = lnA.GetIntersection(lnB, X, dTol);
if(iFlag == Line2D::intBoth) {
// if border is part of the polygon, then there is no doubt
// the intersection exists (it may only touch)
if(bBorder)
return true;
// if the border is not part of the polygon, then we must
// check if lines only touches
else {
if(lnA.GetPosition(X, dTol) & Line2D::posEnd ||
lnB.GetPosition(X, dTol) & Line2D::posEnd) {
// they only touch, this is not intersection
; // do nothing
}
else {
return true;
}
}
}
}
}
return false;
}
double Triangle2D::distance(const ofxVec2f &v) {
int sameHalfPlanes = 0;
Line2D *line;
ofxVec2f end;
if (pAB.positionTo(v) == pSign) {
sameHalfPlanes++;
}
else {
line = &pAB;
end = pBC.getOrigin();
}
if (pBC.positionTo(v) == pSign) {
sameHalfPlanes++;
}
else {
line = &pBC;
end = pCA.getOrigin();
}
if (pCA.positionTo(v) == pSign) {
sameHalfPlanes++;
}
else {
line = &pCA;
end = pAB.getOrigin();
}
if (sameHalfPlanes == 3) {
// point is inside the triangle
return 0;
}
else if (sameHalfPlanes == 2) {
// point could have a projection on a side
ofxVec2f vProjected = line->projectTo(v);
bool vProjectedInSegment =
((line->getOrigin().x <= vProjected.x && vProjected.x <= end.x)
|| (line->getOrigin().x >= vProjected.x && vProjected.x >= end.x))
&&
((line->getOrigin().y <= vProjected.y && vProjected.y <= end.y)
|| (line->getOrigin().y >= vProjected.y && vProjected.y >= end.y));
if (vProjectedInSegment) {
return line->distance(v);
}
}
return MIN(MIN(pAB.getOrigin().distance(v), pBC.getOrigin().distance(v)), pCA.getOrigin().distance(v));
}
int main()
{
for(auto& p : points)
cout << l2.distance(p.coord) << ", ";
cout << endl;
std::multiset<Point> s(points.begin(), points.end());
cout << std::distance(points.begin(), points.end()) << endl;
cout << s.size() << endl;
for(auto& p : s)
cout << p.num << " < ";
cout << endl;
Line2D ll(Coord2D(2, 1), Coord2D(1, 3));
Line2D ly(INFINITY, 0);
Line2D lx(0, 0);
cout << ll.cross(ly).toString() << endl;
cout << ll.cross(lx).toString() << endl;
Line2D l45(Coord2D(0, 0), Vector2D::unit(3.14159/4));
Line2D lv1(Coord2D(1, 0), INFINITY);
cout << Line2D::cross(l45, lv1).toString() << endl;
auto radian = Vector2D(-1, -1).unit().radian();
cout << radian << endl;
cout << Vector2D::unit(radian).toString() << endl;
auto pos = Coord2D(1, 2);
auto dir = Vector2D(-1, -1).unit();
auto length = 5;
Line2D l0(pos, dir);
auto c0x = Line2D::cross(lx, l0);
auto c0y = Line2D::cross(ly, l0);
cout << Line2D::cross(ly, l0).toString() << endl;
cout << Line2D::cross(lx, l0).toString() << endl;
return 0;
}
toxi::geom::Vec2D toxi::geom::Polygon2D::getRandomPoint()
{
std::vector< Line2D > edges = getEdges();
int numEdges = edges.size();
Line2D ea = edges.at( static_cast< int > ( toxi::math::MathUtils::random( numEdges ) ) );
Line2D * eb = nullptr;
// and another one, making sure it's different
while (eb == nullptr || *eb == ea)
{
eb = &edges.at( static_cast< int > ( toxi::math::MathUtils::random( numEdges ) ) );
}
// pick a random point on edge A
Vec2D p = ea.getA().interpolateTo(ea.getB(), toxi::math::MathUtils::random( 1.0 ) );
// then randomly interpolate to another random point on edge B
Vec2D ret = p.interpolateToSelf(
eb->getA().interpolateTo(eb->getB(), toxi::math::MathUtils::random( 1.0 ) ),
static_cast< float > ( toxi::math::MathUtils::random( 1.0 ) ) );
delete eb;
return ret;
}
bool toxi::geom::Polygon2D::toOutLine()
{
int corners = vertices.size();
int maxSegs = corners * 3;
std::vector<Vec2D> newVerts;
std::vector< Vec2D > segments( maxSegs );
std::vector< Vec2D > segEnds( maxSegs );
std::vector< double > segAngles( maxSegs );
//Vec2D * segments;
//segments = ( Vec2D* ) malloc( sizeof( Vec2D ) * maxSegs);
//Vec2D * segEnds;
//segEnds = (Vec2D * ) malloc(sizeof( Vec2D ) * maxSegs );
//float * segAngles;
//segAngles = (float * ) malloc( sizeof( float ) * maxSegs );
//Vec2D[] segments = new Vec2D[maxSegs];
//Vec2D[] segEnds = new Vec2D[maxSegs];
//float[] segAngles = new float[maxSegs];
Vec2D start = vertices.at(0);
double lastAngle = toxi::math::MathUtils::PI;
float a, b, c, d, e, f;
double angleDif, bestAngleDif;
int i, j = corners - 1, segs = 0;
if (corners > maxSegs) {
return false;
}
// 1,3. Reformulate the polygon as a set of line segments, and choose a
// starting point that must be on the perimeter.
for (i = 0; i < corners; i++) {
Vec2D pi = vertices.at(i);
Vec2D pj = vertices.at(j);
if (!( pi == pj )) {
segments[segs] = pi;
segEnds[segs++] = pj;
}
j = i;
if (pi.getY() > start.getY() || (pi.getY() == start.getY() && pi.getX() < start.getX())) {
start.set( pi);
}
}
if (segs == 0) {
return false;
}
// 2. Break the segments up at their intersection points.
for (i = 0; i < segs - 1; i++) {
for (j = i + 1; j < segs; j++) {
Line2D li = toxi::geom::Line2D( segments[i], segEnds[i]);
Line2D lj = toxi::geom::Line2D( segments[j], segEnds[j]);
LineIntersection isec = li.intersectLine( lj );
if (isec.getType() == toxi::geom::LineIntersection::Type::INTERSECTING) {
Vec2D ipos = isec.getPos();
if (!( ipos == segments[i] ) && !( ipos == segEnds[i])) {
if (segs == maxSegs) {
return false;
}
segments[segs] = segments[i];
segEnds[segs++] = ipos;
segments[i] = ipos;
}
if (!( ipos == segments[j] ) && !( ipos == segEnds[ j ] ) ) {
if (segs == maxSegs) {
return false;
}
segments[segs] = segments[j];
segEnds[segs++] = ipos;
segments[j] = ipos;
}
}
}
}
// Calculate the angle of each segment.
for (i = 0; i < segs; i++) {
segAngles[i] = segEnds[i].sub( segments[i] ).positiveHeading();
}
// 4. Build the perimeter polygon.
c = static_cast< float > ( start.getX() );
d = static_cast< float > ( start.getY() );
a = c - 1;
b = d;
e = 0;
f = 0;
newVerts.push_back(Vec2D(c, d));
corners = 1;
while (true) {
bestAngleDif = toxi::math::MathUtils::TWO_PI;
for (i = 0; i < segs; i++) {
if (segments[i].getX() == c && segments[i].getY() == d
&& (segEnds[i].getX() != a || segEnds[i].getY() != b)) {
angleDif = lastAngle - segAngles[i];
while (angleDif >= toxi::math::MathUtils::TWO_PI) {
angleDif -= toxi::math::MathUtils::TWO_PI;
}
while (angleDif < 0) {
angleDif += toxi::math::MathUtils::TWO_PI;
}
if (angleDif < bestAngleDif) {
bestAngleDif = angleDif;
e = static_cast< float > ( segEnds[i].getX() );
f = static_cast< float > ( segEnds[i].getY() );
}
}
if (segEnds[i].getX() == c && segEnds[i].getY() == d
&& (segments[i].getX() != a || segments[i].getY() != b)) {
angleDif = lastAngle - segAngles[i] + toxi::math::MathUtils::PI;
while (angleDif >= toxi::math::MathUtils::TWO_PI) {
angleDif -= toxi::math::MathUtils::TWO_PI;
}
while (angleDif < 0) {
angleDif += toxi::math::MathUtils::TWO_PI;
}
if (angleDif < bestAngleDif) {
bestAngleDif = angleDif;
e = static_cast< float > ( segments[i].getX() );
f = static_cast< float > ( segments[i].getY() );
}
}
}
if (corners > 1 && c == newVerts.at(0).getX() && d == newVerts.at(0).getY()
&& e == newVerts.at(1).getX() && f == newVerts.at(1).getY()) {
corners--;
vertices = newVerts;
return true;
}
if (bestAngleDif == toxi::math::MathUtils::TWO_PI || corners == maxSegs) {
return false;
}
lastAngle -= bestAngleDif + toxi::math::MathUtils::PI;
newVerts.push_back(Vec2D(e, f));
corners++;
a = c;
b = d;
c = e;
d = f;
}
}
/*!
*/
Vector2D
Line2D::intersection( const Line2D & line1,
const Line2D & line2 )
{
double tmp = line1.a() * line2.b() - line1.b() * line2.a();
if ( std::fabs( tmp ) < EPSILON )
{
return Vector2D::INVALIDATED;
}
return Vector2D( (line1.b() * line2.c() - line2.b() * line1.c()) / tmp,
(line2.a() * line1.c() - line1.a() * line2.c()) / tmp );
}
/*!
*/
bool
Body_GoToPointDodge::get_dodge_point( const PlayerAgent * agent,
const Vector2D & target,
Vector2D * sol )
{
const WorldModel & wm = agent->world();
if ( wm.self().pos().dist( target )
< ServerParam::i().defaultPlayerSize() * 2.0 )
{
return false;
}
const Line2D line2target( wm.self().pos(), target );
/*--------------------------------------------------------*/
// check players
const AngleDeg target_angle = ( target - wm.self().pos() ).th();
const double dodge_perpend_dist2
= square( ServerParam::i().defaultPlayerSize() * 2.0 + 0.1 );
const double consider_dist_max = 1.5;
double mindist = 100.0;
bool found = false;
PlayerPtrCont::const_iterator it;
const PlayerPtrCont::const_iterator t_end
= wm.teammatesFromSelf().end();
for ( it = wm.teammatesFromSelf().begin();
it != t_end;
++it )
{
if ( (*it)->distFromSelf() > consider_dist_max )
{
break;
}
// this player is near to target trajectry
if ( mindist > (*it)->distFromSelf()
&& line2target.dist2( (*it)->pos() ) < dodge_perpend_dist2
&& (target_angle - (*it)->angleFromSelf()).abs() < 90.0 )
{
mindist = (*it)->distFromSelf();
*sol = (*it)->pos();
found = true;
}
}
const PlayerPtrCont::const_iterator o_end
= wm.opponentsFromSelf().end();
for ( it = wm.opponentsFromSelf().begin();
it != o_end;
++it )
{
if ( (*it)->distFromSelf() > consider_dist_max )
{
break;
}
// this player is near to target trajectry
if ( mindist > (*it)->distFromSelf()
&& line2target.dist2( (*it)->pos() ) < dodge_perpend_dist2
&& ( target_angle - (*it)->angleFromSelf() ).abs() < 90.0 )
{
mindist = (*it)->distFromSelf();
*sol = (*it)->pos();
found = true;
}
}
/*--------------------------------------------------------*/
// check ball
if ( wm.gameMode().type() != GameMode::PlayOn
&& wm.ball().posValid() )
{
if ( wm.ball().distFromSelf() < consider_dist_max
&& line2target.dist( wm.ball().pos() ) < 1.5
&& ( ( target_angle - ( wm.ball().pos()
- wm.self().pos() ).th() ).abs()
< 90.0 ) )
{
if ( mindist > wm.ball().distFromSelf() )
{
*sol = wm.ball().pos();
found = true;
}
}
}
return found;
}
/*!
*/
int
Circle2D::intersection( const Line2D & line,
Vector2D * sol1,
Vector2D * sol2 ) const
{
if ( std::fabs( line.a() ) < EPSILON )
{
if ( std::fabs( line.b() ) < EPSILON )
{
std::cerr << "Circle2D::intersection() illegal line."
<< std::endl;
return 0;
}
// Line: By + C = 0 ---> y = -C/B
// Circle: (x - cx)^2 + (y - cy)^2 = r^2
// --->
double x1 = 0.0, x2 = 0.0;
int n_sol
= QUADRATIC_FOMULA( 1.0,
-2.0 * center().x,
( SQUARE( center().x )
+ SQUARE( line.c() / line.b() + center().y )
- SQUARE( radius() ) ),
x1,
x2 );
if ( n_sol > 0 )
{
double y1 = -line.c() / line.b();
if ( sol1 )
{
sol1->assign( x1, y1 );
}
if ( n_sol > 1 && sol2 )
{
sol2->assign( x2, y1 );
}
}
return n_sol;
}
else
{
// include (fabs(l.b()) < EPSILON) case
// use line & circle formula
// Ax + By + C = 0
// (x - cx)^2 + (y - cy)^2 = r^2
// make y's quadratic formula using these fomula.
double m = line.b() / line.a();
double d = line.c() / line.a();
double a = 1.0 + m * m;
double b = 2.0 * ( -center().y + ( d + center().x ) * m );
double c = SQUARE( d + center().x )
+ SQUARE( center().y )
- SQUARE( radius() );
double y1 = 0.0, y2 = 0.0;
int n_sol = QUADRATIC_FOMULA( a, b, c,
y1, y2 );
if ( n_sol > 0 && sol1 )
{
sol1->assign( line.getX( y1 ), y1 );
}
if ( n_sol > 1 && sol2 )
{
sol2->assign( line.getX( y2 ), y2 );
}
return n_sol;
}
}
// half plane intersection
bool cmpBySlope(Line2D line1, Line2D line2) {
return line1.ang() < line2.ang();
}
