/**
* Sets an end as edge (starting point if none end were defined, ending point otherwise)
* @param vertex1 one of the vertices of the intercepted edge
* @param vertex2 one of the vertices of the intercepted edge
* @return false if all ends were already defined, true otherwise
*/
bool Segment::setEdge(Vertex* vertex1, Vertex* vertex2)
{
Vector point1 = vertex1->getPosition();
Vector point2 = vertex2->getPosition();
Vector edgeDirection(point2.x - point1.x, point2.y - point1.y, point2.z - point1.z);
Line edgeLine(edgeDirection, point1);
if(index==0)
{
startVertex = vertex1;
startType = EDGE;
startPos = line.computeLineIntersection(&edgeLine);
startDist = line.computePointToPointDistance(startPos);
middleType = FACE;
index++;
return true;
}
else if(index==1)
{
endVertex = vertex1;
endType = EDGE;
endPos = line.computeLineIntersection(&edgeLine);
endDist = line.computePointToPointDistance(endPos);
middleType = FACE;
index++;
//the ending point distance should be smaller than starting point distance
if(startDist>endDist)
swapEnds();
return true;
}
else
return false;
}
bool FvWaypoint::FindClosestPoint(const FvVector3 &kDST3, FvVector3 &kIntersection)
{
FvVector3 cen = this->Centre();
FvVector2 src( cen.x, cen.y );
FvVector2 dst( kDST3.x, kDST3.y );
FvVector2 movementVector = dst - src;
FvLine movementLine(src, dst, true);
for (unsigned int i = 0; i < m_kVertexVector.size(); i++)
{
FvVector2 p1 = m_kVertexVector[i].m_kPosition;
FvVector2 p2 = m_kVertexVector[(i + 1) % m_kVertexVector.size()].m_kPosition;
float cp1 = movementVector.CrossProduct(p1 - src);
float cp2 = movementVector.CrossProduct(p2 - src);
if(cp1 > 0.0f && cp2 < 0.0f)
{
FvLine edgeLine(p1, p2, true);
float p = movementLine.Intersect(edgeLine);
FvVector2 interv2 = movementLine.Param(p);
kIntersection.Set( interv2.x, interv2.y, cen.z + 0.1f );
return true;
}
}
return false;
}
MyVertex::Index GetRepVertex_SPOLY(MyEdge::Index edgeId, SubPolygon* polygon)
{
assert(orientation(polygon->supportingPlane(),(polygon->vertex(0))) == ON_ORIENTED_BOUNDARY);
assert(orientation(polygon->supportingPlane(),(polygon->vertex(1))) == ON_ORIENTED_BOUNDARY);
assert(orientation(polygon->supportingPlane(),(polygon->vertex(2))) == ON_ORIENTED_BOUNDARY);
MyEdge& edge = xedge(edgeId);
// find init point
int edgeIndexInFace = -1;
for (int i = 0; i < polygon->degree(); i++)
{
if (polygon->edgeId(i) == edgeId)
{
edgeIndexInFace = i;
break;
}
}
assert(edgeIndexInFace != -1);
MyVertex::Index vIdInPlane;
XPlane boundPlane, tmpPlane;
for (int i = 2; i < polygon->degree(); i++)
{
vIdInPlane = polygon->vertexId((edgeIndexInFace + i) % polygon->degree());
// find a bounding plane
bool flag = false;
for (auto &neigh : *edge.neighbor)
{
if (neigh.type == NeighborInfo::Edge)
{
for (auto fItr = MyEdge::FaceIterator(xedge(neigh.neighborEdgeId), true);
fItr; fItr.incrementToTriangle())
{
assert(fItr.face()->getType() == IPolygon::TRIANGLE);
tmpPlane = ((Triangle*)fItr.face())->supportingPlane();
if (orientation(tmpPlane, xvertex(vIdInPlane)) != ON_ORIENTED_BOUNDARY)
{
boundPlane = tmpPlane;
flag = true;
break;
}
}
}
else
{
assert(neigh.type == NeighborInfo::Face);
XPlane tmpPlane = neigh.pTrangle->supportingPlane();
if (orientation(tmpPlane, xvertex(vIdInPlane)) != ON_ORIENTED_BOUNDARY)
{
boundPlane = tmpPlane;
break;
}
}
if (flag) break;
}
if (boundPlane.isValid()) break;
}
assert(boundPlane.isValid());
// correct the direction of bounding plane
XLine edgeLine(polygon->supportingPlane(), boundPlane);
assert(!polygon->supportingPlane().idEquals(boundPlane));
int tmpSide = linearOrder(edgeLine, xvertex(polygon->vertexId((edgeIndexInFace + 1) % polygon->degree())),
xvertex(polygon->vertexId(edgeIndexInFace)));
assert(tmpSide != 0);
if (tmpSide < 0)
boundPlane.inverse();
// pick a correct rep vertex
MyVertex::Index repVertexId = INVALID_UINT32;
for (size_t i = 0; i < polygon->degree(); i++)
{
if (orientation(boundPlane, xvertex(polygon->vertexId(i))) == ON_POSITIVE_SIDE)
{
repVertexId = polygon->vertexId(i);
break;
}
}
assert(repVertexId != INVALID_UINT32);
return repVertexId;
}
/**
* This method fills in the adjacentState reference with
* the state for a given adjacency.
*
* @param index Index of adjacency
* @param adjacency State of adjacency is copied here
* @param goal Goal state
*
* @return True if this is a valid adjacency.
*/
bool WaypointState::getAdjacency(int index,
WaypointState& adjacency, const WaypointGoalState& goal) const
{
Vector2 src, dst, p1, p2, movementVector, next;
float p, cp1, cp2;
// If this edge on waypoint is not passable, forget it.
const WaypointSet * pAdjSet = pWaypoint_->adjacentWaypointSet(index);
if (pWaypoint_->adjacentWaypoint(index) == NULL && pAdjSet == NULL)
return false;
// We need 2d vectors for intersection tests.
src.x = position_.x;
src.y = position_.z;
dst.x = goal.position_.x;
dst.y = goal.position_.z;
movementVector = dst - src;
p1 = pWaypoint_->vertexPosition(index);
p2 = pWaypoint_->vertexPosition((index + 1) % pWaypoint_->vertexCount());
// move the pts towards each other if we have extra radius
if (goal.extraRadius() > 0.f)
{
const float ger = goal.extraRadius();
Vector2 edir( p2 - p1 );
const float elen = edir.length();
// for now can only pass edges that are long enough
// (need to only move in conditionally, e.g. if adj to black)
if (elen < ger * 2.f) return false;
edir *= ger / elen;
p1 += edir;
p2 -= edir;
}
cp1 = movementVector.crossProduct(p1 - src);
cp2 = movementVector.crossProduct(p2 - src);
// If our desired path takes us through this line segment,
// find the intersection and use it. Otherwise use the
// vertex whose crossproduct is closest to zero.
if(cp1 > 0.0f && cp2 < 0.0f)
{
LineEq moveLine(src, dst, true);
LineEq edgeLine(p1, p2, true);
p = moveLine.intersect(edgeLine);
next = moveLine.param(p);
}
else if(fabs(cp1) < fabs(cp2))
{
next = p1;
}
else
{
next = p2;
}
adjacency.pWPSet_ = (pAdjSet == NULL) ? pWPSet_ : pAdjSet;
adjacency.waypointID_ = pWaypoint_->adjacentID(index);
adjacency.pWaypoint_ = pWaypoint_->adjacentWaypoint(index);
adjacency.position_.x = next.x;
adjacency.position_.y = position_.y;
adjacency.position_.z = next.y;
adjacency.distanceFromParent_ = (position_ - adjacency.position_).length();
return true;
}
