int ClosedPolygon::insertIndexedPoint(const Vec3d &p)
{
int vIndex = -1;
kdres * findP = points.nearest3f(p.x(), p.y(), p.z());
if(findP)
{
double * pos = findP->riter->item->pos;
Vec3d closestPoint(pos[0], pos[1], pos[2]);
double dist = (closestPoint - p).norm();
if(dist < closedPolyEpsilon)
{
vIndex = findP->riter->item->index;
kd_res_free(findP);
return vIndex;
}
}
vIndex = lastVertexIndex;
allPoints[vIndex] = p;
points.insert3f(p.x(), p.y(), p.z(), lastVertexIndex++);
return vIndex;
}
RS_Vector RS_EntityContainer::getNearestRef(const RS_Vector& coord,
double* dist) {
double minDist = RS_MAXDOUBLE; // minimum measured distance
double curDist; // currently measured distance
RS_Vector closestPoint(false); // closest found endpoint
RS_Vector point; // endpoint found
for (RS_Entity* en = firstEntity();
en != NULL;
en = nextEntity()) {
if (en->isVisible()) {
point = en->getNearestRef(coord, &curDist);
if (point.valid && curDist<minDist) {
closestPoint = point;
minDist = curDist;
if (dist!=NULL) {
*dist = curDist;
}
}
}
}
return closestPoint;
}
bool dgApi dgPointToPolygonDistance (const dgVector& p, const dgFloat32* const polygon, dgInt32 strideInBytes,
const dgInt32* const indexArray, dgInt32 indexCount, dgFloat32 bailDistance, dgVector& out)
{
_ASSERTE (0);
dgInt32 stride = dgInt32 (strideInBytes / sizeof (dgFloat32));
dgInt32 i0 = indexArray[0] * stride;
dgInt32 i1 = indexArray[1] * stride;
const dgVector v0 (&polygon[i0]);
dgVector v1 (&polygon[i1]);
dgVector closestPoint (dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f), dgFloat32 (0.0f));
dgFloat32 minDist = dgFloat32 (1.0e20f);
for (dgInt32 i = 2; i < indexCount; i ++) {
dgInt32 i2 = indexArray[i] * stride;
const dgVector v2 (&polygon[i2]);
const dgVector q (dgPointToTriangleDistance (p, v0, v1, v2));
const dgVector error (q - p);
dgFloat32 dist = error % error;
if (dist < minDist) {
minDist = dist;
closestPoint = q;
}
v1 = v2;
}
if (minDist > (bailDistance * bailDistance)) {
return false;
}
out = closestPoint;
return true;
}
C3dWorldPoint C3dLine::closestPointToBoundedLine(const C3dBoundedLine & boundedLine) const
{
//return closestPointToBoundedLine_int<C3dBoundedLine>(boundedLine);
const C3dWorldPoint closestUnbounded = closestPointToLine(boundedLine.unboundedLine());
C3dWorldPoint closestPointOnBounded = boundedLine.closestPoint(closestUnbounded);
return closestPoint(closestPointOnBounded);
}
/// \param point Point to flip of the plane
/// \return The point passed in flipped over the plane
Vector3 Plane::reflectPoint(const Vector3& point) const
{
Vector3 out;
Vector3 closest;
closest = closestPoint(point);
out = 2.0f * closest - point;
return out;
}
Vect2 VectFuns::closestPointOnSegment(const Vect2& a, const Vect2& b, const Vect2& so) {
Vect2 i = closestPoint(a,b,so);
double d1 = a.distance(b);
double d2 = a.distance(i);
double d3 = b.distance(i);
if (d2 <= d1 && d3 <= d1) {
return i;
} else if (d2 < d3) {
return a;
} else {
return b;
}
}
void HandController::update() {
/*HandData h = manager->getSkelData();
h.getData(HandData::LEFT_TIP, hand[0]);
h.getData(HandData::RIGHT_TIP, hand[1]);
updateHand(0, h);
updateHand(1, h);*/
state[0] = TRACKING;
closestPoint();
if (state[0] == ENGAGED) {
meshes[0]->setTranslation(hand[0]);
}
}
// -----------------------------------------------------------------
// Name : closestDistanceTo
// Returns the closest distance between the polygon and the input point
// -----------------------------------------------------------------
double Polygon::closestDistanceTo(f3d point)
{
double closestDistance = -1;
f3d vertA = *(vertice.rbegin());
for (f3d vertB : vertice) {
f3d closest = closestPoint(point, vertA, vertB, true/*segment*/);
double distance2 = (point-closest).getSize2();
if (closestDistance < 0 || distance2 < closestDistance) {
closestDistance = distance2;
}
vertA = vertB;
}
return sqrt(closestDistance);
}
C2dImagePointPx C2dLine::closestPointToBoundedLine(const C2dBoundedLine & boundedLine) const
{
//return closestPointToBoundedLine_int<C2dBoundedLine>(boundedLine);
optional<const C2dImagePointPx> intersection = findIntersection(boundedLine.unboundedLine());
if(intersection)
{
//If intersection is on the bounded line, return intersection, else return whichever point on this line is closest to the bounded line endpoint
C2dImagePointPx closestPointOnBounded = boundedLine.closestPoint(*intersection);
return closestPoint(closestPointOnBounded);
}
else
{
return getPointOnLine();
}
}
/**
* @return The intersection which is closest to 'coord'
*/
RS_Vector RS_EntityContainer::getNearestIntersection(const RS_Vector& coord,
double* dist) {
double minDist = RS_MAXDOUBLE; // minimum measured distance
double curDist; // currently measured distance
RS_Vector closestPoint(false); // closest found endpoint
RS_Vector point; // endpoint found
RS_VectorSolutions sol;
RS_Entity* closestEntity;
closestEntity = getNearestEntity(coord, NULL, RS2::ResolveAll);
if (closestEntity!=NULL) {
for (RS_Entity* en = firstEntity(RS2::ResolveAll);
en != NULL;
en = nextEntity(RS2::ResolveAll)) {
if (en->isVisible() && en!=closestEntity) {
sol = RS_Information::getIntersection(closestEntity,
en,
true);
for (int i=0; i<4; i++) {
point = sol.get(i);
if (point.valid) {
curDist = coord.distanceTo(point);
if (curDist<minDist) {
closestPoint = point;
minDist = curDist;
if (dist!=NULL) {
*dist = curDist;
}
}
}
}
}
}
//}
}
return closestPoint;
}
AxisSet* Circle::getSeparatingAxes(const Shape* s) const
{
AxisSet *as = new AxisSet();
std::auto_ptr<OrderedPair> closestPoint(s->getClosestVertex(getCentre()));
if (s->isVertex(*closestPoint))
{
std::auto_ptr<OrderedPair> closestPointToVertex(getClosestVertex(*closestPoint));
Vector separatingAxis = Vector(*closestPointToVertex)
- Vector(*closestPoint);
if (!(separatingAxis == Vector(0, 0)))
{
as->add(separatingAxis.Normalise());
}
}
return as;
}
bool GEdge::computeDistanceFromMeshToGeometry (double &d2, double &dmax)
{
d2 = 0.0; dmax = 0.0;
if (geomType() == Line) return true;
if (!lines.size())return false;
IntPt *pts;
int npts;
lines[0]->getIntegrationPoints(2*lines[0]->getPolynomialOrder(), &npts, &pts);
for (unsigned int i = 0; i < lines.size(); i++){
MLine *l = lines[i];
double t[256];
for (int j=0; j< l->getNumVertices();j++){
MVertex *v = l->getVertex(j);
if (v->onWhat() == getBeginVertex()){
t[j] = getLowerBound();
}
else if (v->onWhat() == getEndVertex()){
t[j] = getUpperBound();
}
else {
v->getParameter(0,t[j]);
}
}
for (int j=0;j<npts;j++){
SPoint3 p;
l->pnt(pts[j].pt[0],0,0,p);
double tinit = l->interpolate(t,pts[j].pt[0],0,0);
GPoint pc = closestPoint(p, tinit);
if (!pc.succeeded())continue;
double dsq =
(pc.x()-p.x())*(pc.x()-p.x()) +
(pc.y()-p.y())*(pc.y()-p.y()) +
(pc.z()-p.z())*(pc.z()-p.z());
d2 += pts[i].weight * fabs(l->getJacobianDeterminant(pts[j].pt[0],0,0)) * dsq;
dmax = std::max(dmax,sqrt(dsq));
}
}
d2 = sqrt(d2);
return true;
}
double computeDistanceLeafCamera(Leaf& l, glm::vec3 camPosition, float terrainScale){
float scaledLeafSize = l.size*terrainScale;
/* get the relative position of the camera in comparison of leaf origin */
glm::vec3 rel_camPosition = camPosition - glm::vec3(l.pos.x*terrainScale, l.pos.y*terrainScale, l.pos.z*terrainScale);
/* Declare closest point on leaf cube */
glm::vec3 closestPoint(0.f,0.f,0.f);
/* Get the closest x coordinate*/
if(rel_camPosition.x < 0.f){
closestPoint.x = 0.f;
}else if(rel_camPosition.x > scaledLeafSize){
closestPoint.x = scaledLeafSize;
}else{
closestPoint.x = rel_camPosition.x;
}
/* Get the closest y coordinate*/
if(rel_camPosition.y < 0.f){
closestPoint.y = 0.f;
}else if(rel_camPosition.y > scaledLeafSize){
closestPoint.y = scaledLeafSize;
}else{
closestPoint.y = rel_camPosition.y;
}
/* Get the closest z coordinate*/
if(rel_camPosition.z < 0.f){
closestPoint.z = 0.f;
}else if(rel_camPosition.z > scaledLeafSize){
closestPoint.z = scaledLeafSize;
}else{
closestPoint.z = rel_camPosition.z;
}
/* case where the camera is inside the cube */
if(closestPoint == rel_camPosition){
return 0;
}
return glm::distance(closestPoint, rel_camPosition);
}
Vect3 VectFuns::closestPoint(const Vect3& a, const Vect3& b, const Vect3& so) {
if (a.almostEquals(b)) return Vect3::INVALID();
Vect2 c = closestPoint(a.vect2(), b.vect2(), so.vect2());
Vect3 v = b.Sub(a);
double d1 = v.vect2().norm();
double d2 = c.Sub(a.vect2()).norm();
double d3 = c.Sub(b.vect2()).norm();
double f = d2/d1;
if (d3 > d1 && d3 > d2) { // negative direction
f = -f;
}
return a.AddScal(f, v);
// Vect3 v = a.Sub(b).PerpL().Hat2D(); // perpendicular vector to line
// Vect3 s2 = so.AddScal(100, v);
// std::pair<Vect3, double> i = intersectionAvgZ(a,b,100,so,s2);
// // need to fix altitude to be along the a-b line
// return interpolate(a,b,i.second/100.0);
}
/**
* @return The intersection which is closest to 'coord'
*/
RS_Vector RS_EntityContainer::getNearestIntersection(const RS_Vector& coord,
double* dist) {
double minDist = RS_MAXDOUBLE; // minimum measured distance
double curDist = RS_MAXDOUBLE; // currently measured distance
RS_Vector closestPoint(false); // closest found endpoint
RS_Vector point; // endpoint found
RS_VectorSolutions sol;
RS_Entity* closestEntity;
closestEntity = getNearestEntity(coord, nullptr, RS2::ResolveAllButTextImage);
if (closestEntity) {
for (RS_Entity* en = firstEntity(RS2::ResolveAllButTextImage);
en;
en = nextEntity(RS2::ResolveAllButTextImage)) {
if (
!en->isVisible()
|| en->getParent()->ignoredOnModification()
){
continue;
}
sol = RS_Information::getIntersection(closestEntity,
en,
true);
point=sol.getClosest(coord,&curDist,nullptr);
if(sol.getNumber()>0 && curDist<minDist){
closestPoint=point;
minDist=curDist;
}
}
}
if(dist && closestPoint.valid) {
*dist = minDist;
}
return closestPoint;
}
/**
* @return The point which is closest to 'coord'
* (one of the vertexes)
*/
RS_Vector RS_EntityContainer::getNearestEndpoint(const RS_Vector& coord,
double* dist, RS_Entity** pEntity)const {
double minDist = RS_MAXDOUBLE; // minimum measured distance
double curDist; // currently measured distance
RS_Vector closestPoint(false); // closest found endpoint
RS_Vector point; // endpoint found
//QListIterator<RS_Entity> it = createIterator();
//RS_Entity* en;
//while ( (en = it.current()) ) {
// ++it;
unsigned i0=0;
for(auto en: entities){
if (!en->getParent()->ignoredOnModification() ){//no end point for Insert, text, Dim
// std::cout<<"find nearest for entity "<<i0<<std::endl;
point = en->getNearestEndpoint(coord, &curDist);
if (point.valid && curDist<minDist) {
closestPoint = point;
minDist = curDist;
if (dist) {
*dist = minDist;
}
if(pEntity){
*pEntity=en;
}
}
}
i0++;
}
// std::cout<<__FILE__<<" : "<<__func__<<" : line "<<__LINE__<<std::endl;
// std::cout<<"count()="<<const_cast<RS_EntityContainer*>(this)->count()<<"\tminDist= "<<minDist<<"\tclosestPoint="<<closestPoint;
// if(pEntity ) std::cout<<"\t*pEntity="<<*pEntity;
// std::cout<<std::endl;
return closestPoint;
}
RS_Vector RS_EntityContainer::getNearestSelectedRef(const RS_Vector& coord,
double* dist) const{
double minDist = RS_MAXDOUBLE; // minimum measured distance
double curDist; // currently measured distance
RS_Vector closestPoint(false); // closest found endpoint
RS_Vector point; // endpoint found
for(auto en: entities){
if (en->isVisible() && en->isSelected() && !en->isParentSelected()) {
point = en->getNearestSelectedRef(coord, &curDist);
if (point.valid && curDist<minDist) {
closestPoint = point;
minDist = curDist;
if (dist) {
*dist = minDist;
}
}
}
}
return closestPoint;
}
void ObstacleCombinator::generateObstacleFromCurrentCluster(std::vector<ObstacleModel::Obstacle>& obstacles)
{
Vector2<int>& c = currentCluster.cells[0];
int xMin(c.x);
int yMin(c.y);
int xMax(c.x);
int yMax(c.y);
float rightAngle = 10.0;
float leftAngle = -10.0;
Vector2<int> rightCorner;
Vector2<int> leftCorner;
Vector2<> centerCells;
Vector2<int> robotPosition(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2);
Vector2<int> closestPoint(USObstacleGrid::GRID_LENGTH, USObstacleGrid::GRID_LENGTH);
int closestPointSqrDist(USObstacleGrid::GRID_SIZE * 2);
for(unsigned int i = 0; i < currentCluster.cells.size(); ++i)
{
c = currentCluster.cells[i];
centerCells.x += c.x;
centerCells.y += c.y;
Vector2<int> point(c.x - robotPosition.x, c.y - robotPosition.y);
int sqrDistToRobot = sqr(point.x) + sqr(point.y);
float angleToRobot = atan2(static_cast<float>(point.y), static_cast<float>(point.x));
if(angleToRobot < rightAngle)
{
rightAngle = angleToRobot;
rightCorner = Vector2<int>(c.x, c.y);
}
if(angleToRobot > leftAngle)
{
leftAngle = angleToRobot;
leftCorner = Vector2<int>(c.x, c.y);
}
if(sqrDistToRobot < closestPointSqrDist)
{
closestPoint = c;
closestPointSqrDist = sqrDistToRobot;
}
if(c.x < xMin)
xMin = c.x;
else if(c.x > xMax)
xMax = c.x;
if(c.y < yMin)
yMin = c.y;
else if(c.y > yMax)
yMax = c.y;
}
centerCells /= static_cast<float>(currentCluster.cells.size());
const float angleToCenterPoint = Geometry::angleTo(Pose2D(USObstacleGrid::GRID_LENGTH / 2, USObstacleGrid::GRID_LENGTH / 2), centerCells); //calculates the angle to the center of the cluster in grid coordinate system (independent of robot rotation)
const float cosinus = cos(-angleToCenterPoint);
const float sinus = sin(-angleToCenterPoint);
float newX;
float newY;
//initializing of the rectangle
float xMinRotated(FLT_MAX);
float yMinRotated(FLT_MAX);
float xMaxRotated(-FLT_MAX);
float yMaxRotated(-FLT_MAX);
for(unsigned int i = 0; i < currentCluster.cells.size(); ++i)
{
newX = cosinus * currentCluster.cells[i].x - sinus * currentCluster.cells[i].y; // rotates each cell of the cluster
newY = sinus * currentCluster.cells[i].x + cosinus * currentCluster.cells[i].y;
//sets new values for rectangle
if(newX < xMinRotated)
xMinRotated = newX;
else if(newX > xMaxRotated)
xMaxRotated = newX;
if(newY < yMinRotated)
yMinRotated = newY;
else if(newY > yMaxRotated)
yMaxRotated = newY;
}
Vector2<> closestPointWorld = gridToWorld(Vector2<>(closestPoint.x + 0.5f, closestPoint.y + 0.5f));
Vector2<> centerWorld = gridToWorld(centerCells);
//expansion (length of x- and y-axis through the center point) and orientation (dependent on robot rotation) of the cluster
Vector3<> covarianceEllipse(((xMaxRotated - xMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, ((yMaxRotated - yMinRotated) * USObstacleGrid::CELL_SIZE) * parameters.covarianceFactor, atan2(centerWorld.y, centerWorld.x));
Matrix2x2<> covariance(covarianceEllipse.x, 0, 0, covarianceEllipse.y); // covariance is initialised with uncorrelated values (only expansion of cluster as variances)
rotateMatrix(covariance, covarianceEllipse.z); // rotates the covariance so that it fits to the orientation and expansion of the cluster
obstacles.push_back(ObstacleModel::Obstacle(gridToWorld(Vector2<>((float)(leftCorner.x), (float)(leftCorner.y))),
gridToWorld(Vector2<>((float)(rightCorner.x), (float)(rightCorner.y))), centerWorld, closestPointWorld,
static_cast<int>(currentCluster.cells.size()), covariance));
}
/**
* Computes the closest points on two line segments.
* @param p the point to find the closest point to
* @return a pair of Coordinates which are the closest points on the line segments
* The returned CoordinateSequence must be delete by the caller
*/
CoordinateSequence* LineSegment::closestPoints(const LineSegment *line){
// test for intersection
Coordinate *intPt = intersection(line);
if (intPt!=NULL) {
CoordinateSequence *cl=new DefaultCoordinateSequence(new vector<Coordinate>(2, *intPt));
//cl->add(*intPt);
//cl->add(*intPt);
delete intPt;
return cl;
}
/*
* if no intersection closest pair contains at least one endpoint.
* Test each endpoint in turn.
*/
CoordinateSequence *closestPt=new DefaultCoordinateSequence(2);
//vector<Coordinate> *cv = new vector<Coordinate>(2);
double minDistance=DoubleInfinity;
double dist;
Coordinate *close00 = closestPoint(line->p0);
minDistance = close00->distance(line->p0);
closestPt->setAt(*close00,0);
//(*cv)[0] = *close00;
delete close00;
closestPt->setAt(line->p0,1);
//(*cv)[1] = line->p0;
Coordinate *close01 = closestPoint(line->p1);
dist = close01->distance(line->p1);
if (dist < minDistance) {
minDistance = dist;
closestPt->setAt(*close01,0);
closestPt->setAt(line->p1,1);
//(*cv)[0] = *close01;
//(*cv)[1] = line->p1;
}
delete close01;
Coordinate *close10 = line->closestPoint(p0);
dist = close10->distance(p0);
if (dist < minDistance) {
minDistance = dist;
closestPt->setAt(p0,0);
closestPt->setAt(*close10,1);
//(*cv)[0] = p0;
//(*cv)[1] = *close10;
}
delete close10;
Coordinate *close11 = line->closestPoint(p1);
dist = close11->distance(p1);
if (dist < minDistance) {
minDistance = dist;
closestPt->setAt(p1,0);
closestPt->setAt(*close11,1);
//(*cv)[0] = p1;
//(*cv)[1] = *close11;
}
delete close11;
//CoordinateSequence *closestPt=new DefaultCoordinateSequence(cv);
return closestPt;
}
double distance2(const tri<ndim>& tri, const vector<ndim>& pt) {
return distance2(closestPoint(tri, pt), pt);
}
CoordinateSequence*
LineSegment::closestPoints(const LineSegment& line)
{
// test for intersection
Coordinate intPt;
if ( intersection(line, intPt) )
{
CoordinateSequence *cl=new CoordinateArraySequence(new vector<Coordinate>(2, intPt));
return cl;
}
/*
* if no intersection closest pair contains at least one endpoint.
* Test each endpoint in turn.
*/
CoordinateSequence *closestPt=new CoordinateArraySequence(2);
//vector<Coordinate> *cv = new vector<Coordinate>(2);
double minDistance=DoubleMax;
double dist;
Coordinate close00;
closestPoint(line.p0, close00);
minDistance = close00.distance(line.p0);
closestPt->setAt(close00, 0);
closestPt->setAt(line.p0, 1);
Coordinate close01;
closestPoint(line.p1, close01);
dist = close01.distance(line.p1);
if (dist < minDistance) {
minDistance = dist;
closestPt->setAt(close01,0);
closestPt->setAt(line.p1,1);
//(*cv)[0] = close01;
//(*cv)[1] = line.p1;
}
Coordinate close10;
line.closestPoint(p0, close10);
dist = close10.distance(p0);
if (dist < minDistance) {
minDistance = dist;
closestPt->setAt(p0,0);
closestPt->setAt(close10,1);
//(*cv)[0] = p0;
//(*cv)[1] = close10;
}
Coordinate close11;
line.closestPoint(p1, close11);
dist = close11.distance(p1);
if (dist < minDistance) {
minDistance = dist;
closestPt->setAt(p1,0);
closestPt->setAt(close11,1);
//(*cv)[0] = p1;
//(*cv)[1] = *close11;
}
return closestPt;
}
C2dImagePointPx C2dBoundedLine::closestPointToLine(const C2dLine & otherLine) const
{
const C2dImagePointPx intersect = *(unboundedLine().findIntersection(otherLine));
return closestPoint(intersect);
}
/** Get the square of the distance of a point from the ray. */
T squaredDistance(VectorT const & p) const { return (closestPoint(p) - p).squaredLength(); }
/** Get the distance of a point from the ray. */
T distance(VectorT const & p) const { return (closestPoint(p) - p).length(); }
C3dWorldPoint C3dBoundedLine::closestPointToBoundedLine(const C3dBoundedLine & boundedLine) const
{
return closestPoint(C3dLine(unboundedLine()).closestPointToBoundedLine(boundedLine));
}
C3dWorldPoint C3dBoundedLine::closestPointToLine(const C3dLine & otherLine) const
{
const C3dWorldPoint closestOnOther = otherLine.closestPointToBoundedLine(*this);
return closestPoint(closestOnOther);
}
/**
* Update the simplex and return closest point to origin on the simplex
* @return Closest point to origin on the simplex
*/
template<class T> void GJKAlgorithm<T>::updateSimplex(Simplex<T> &simplex) const
{
Point3<T> closestPoint(0.0, 0.0, 0.0);
T barycentrics[4];
if(simplex.getSize() == 2)
{ //simplex is a line (1D)
const Point3<T> &pointA = simplex.getPoint(0);
const Point3<T> &pointB = simplex.getPoint(1); //pointB is the last point added to the simplex
closestPoint = LineSegment3D<T>(pointA, pointB).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics);
simplex.setBarycentric(0, barycentrics[0]);
simplex.setBarycentric(1, barycentrics[1]);
}else if(simplex.getSize() == 3)
{ //simplex is a triangle (2D)
const Point3<T> &pointA = simplex.getPoint(0);
const Point3<T> &pointB = simplex.getPoint(1);
const Point3<T> &pointC = simplex.getPoint(2); //pointC is the last point added to the simplex
const Vector3<T> co = pointC.vector(Point3<T>(0.0, 0.0, 0.0));
const Vector3<T> cb = pointC.vector(pointB);
const Vector3<T> ca = pointC.vector(pointA);
const Vector3<T> normalAbc = cb.crossProduct(ca);
closestPoint = Triangle3D<T>(pointA, pointB, pointC).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics);
simplex.setBarycentric(0, barycentrics[0]);
simplex.setBarycentric(1, barycentrics[1]);
simplex.setBarycentric(2, barycentrics[2]);
if(barycentrics[1]==0.0)
{ //remove pointB
simplex.removePoint(1);
}
if(barycentrics[0]==0.0)
{ //remove pointA
simplex.removePoint(0);
}
if(normalAbc.dotProduct(co) <= 0.0)
{ //voronoi region -ABC => ABC
simplex.swapPoints(0, 1); //swap pointA and pointB
}
}else if (simplex.getSize() == 4)
{ //simplex is a tetrahedron (3D)
const Point3<T> &pointA = simplex.getPoint(0);
const Point3<T> &pointB = simplex.getPoint(1);
const Point3<T> &pointC = simplex.getPoint(2);
const Point3<T> &pointD = simplex.getPoint(3); //pointD is the last point added to the simplex
const short voronoiRegionMask = 14; //test all voronoi regions except the one which doesn't include the new point added (pointD)
closestPoint = Tetrahedron<T>(pointA, pointB, pointC, pointD).closestPoint(Point3<T>(0.0, 0.0, 0.0), barycentrics, voronoiRegionMask);
simplex.setBarycentric(0, barycentrics[0]);
simplex.setBarycentric(1, barycentrics[1]);
simplex.setBarycentric(2, barycentrics[2]);
simplex.setBarycentric(3, barycentrics[3]);
if(barycentrics[2]==0.0)
{ //remove pointC
simplex.removePoint(2);
}
if(barycentrics[1]==0.0)
{ //remove pointB
simplex.removePoint(1);
}
if(barycentrics[0]==0.0)
{ //remove pointA
simplex.removePoint(0);
}
}else
{
std::ostringstream oss;
oss << simplex.getSize();
throw std::invalid_argument("Size of simplex unsupported: " + oss.str() + ".");
}
simplex.setClosestPointToOrigin(closestPoint);
}
float sqrDistance(const LineSegment3& x, const Vector3& q)
{
return sqrDistance(closestPoint(x, q), q);
}
float LineSegment2D::distance(const Vector2& p) const {
Vector2 closest = closestPoint(p);
return (closest - p).length();
}
C2dImagePointPx C2dBoundedLine::closestPointToBoundedLine(const C2dBoundedLine & boundedLine) const
{
return closestPoint(C2dLine(unboundedLine()).closestPointToBoundedLine(boundedLine));
}
