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)); }