You cannot select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
233 lines
6.9 KiB
C++
233 lines
6.9 KiB
C++
#include "Line6f.h"
|
|
|
|
|
|
|
|
Line6f::Line6f(const Vector3f &polar_s, const Vector3f &polar_e) :
|
|
startp(polar_s), endp(polar_e), startc(polar_s.toCartesian()), endc(polar_e.toCartesian()), length(startc.dist(endc)) {};
|
|
|
|
Line6f::Line6f(const Vector3f &cart_s, const Vector3f &cart_e, float length) :
|
|
startp(cart_s.toPolar()), endp(cart_e.toPolar()), startc(cart_s), endc(cart_e), length(length) {};
|
|
|
|
Line6f::Line6f(const Line6f &obj) :
|
|
startp(obj.startp), endp(obj.endp), startc(obj.startc), endc(obj.endc), length(obj.length) {};
|
|
|
|
Vector3f Line6f::linePointClosestToCartPoint(const Vector3f &cp) const{
|
|
|
|
/**
|
|
* Equation of this line: (we want to find t, such that (cp-p) is perpendicular to this line)
|
|
* p = startc + t*(endc - startc)
|
|
*
|
|
* Let vecp=(cp-start) and vecline=(end-start)
|
|
* Scalar projection of vecp in the direction of vecline:
|
|
* sp = vecp.vecline/|vecline|
|
|
* Find the ratio t by dividing the scalar projection by the length of vecline:
|
|
* t = sp / |vecline|
|
|
* So the final expression becomes:
|
|
* t = vecp.vecline/|vecline|^2
|
|
*/
|
|
|
|
|
|
Vector3f lvec(endc-startc); //this line's vector
|
|
float t = (cp-startc).innerProduct(lvec) / (length*length);
|
|
|
|
return startc + lvec*t;
|
|
}
|
|
|
|
Vector3f Line6f::linePointClosestToPolarPoint(const Vector3f &pp) const{
|
|
return linePointClosestToCartPoint(pp.toCartesian());
|
|
}
|
|
|
|
float Line6f::lineDistToCartPoint(const Vector3f &cp) const{
|
|
return ((cp-startc).crossProduct(cp-endc)).length() / (endc-startc).length();
|
|
}
|
|
|
|
float Line6f::lineDistToPolarPoint(const Vector3f &pp) const{
|
|
return lineDistToCartPoint(pp.toCartesian());
|
|
}
|
|
|
|
//source: http://geomalgorithms.com/a07-_distance.html#dist3D_Line_to_Line()
|
|
float Line6f::lineDistToLine(const Line6f &l) const{
|
|
Vector3f u = endc - startc;
|
|
Vector3f v = l.endc - l.startc;
|
|
Vector3f w = startc - l.startc;
|
|
float a = u.innerProduct(u); // always >= 0
|
|
float b = u.innerProduct(v);
|
|
float c = v.innerProduct(v); // always >= 0
|
|
float d = u.innerProduct(w);
|
|
float e = v.innerProduct(w);
|
|
float D = a*c - b*b; // always >= 0
|
|
float sc, tc;
|
|
|
|
// compute the line parameters of the two closest points
|
|
if (D < 1e-8f) { // the lines are almost parallel
|
|
sc = 0.0;
|
|
tc = (b>c ? d/b : e/c); // use the largest denominator
|
|
}
|
|
else {
|
|
sc = (b*e - c*d) / D;
|
|
tc = (a*e - b*d) / D;
|
|
}
|
|
|
|
// get the difference of the two closest points
|
|
Vector3f dP = w + (u * sc) - (v * tc); // = L1(sc) - L2(tc)
|
|
|
|
return dP.length(); // return the closest distance
|
|
}
|
|
|
|
Vector3f Line6f::segmentPointClosestToCartPoint(const Vector3f &cp) const{
|
|
|
|
/**
|
|
* Equation of this line: (we want to find t, such that (cp-p) is perpendicular to this line)
|
|
* p = startc + t*(endc - startc)
|
|
*
|
|
* Let vecp=(cp-start) and vecline=(end-start)
|
|
* Scalar projection of vecp in the direction of vecline:
|
|
* sp = vecp.vecline/|vecline|
|
|
* Find the ratio t by dividing the scalar projection by the length of vecline:
|
|
* t = sp / |vecline|
|
|
* So the final expression becomes:
|
|
* t = vecp.vecline/|vecline|^2
|
|
* Since this version requires that p belongs to the line segment, there is an additional restriction:
|
|
* 0 < t < 1
|
|
*/
|
|
|
|
Vector3f lvec(endc-startc); //this line's vector
|
|
float t = (cp-startc).innerProduct(lvec) / (length*length);
|
|
|
|
if(t<0) t=0;
|
|
else if(t>1) t=1;
|
|
|
|
return startc + lvec*t;
|
|
}
|
|
|
|
Vector3f Line6f::segmentPointClosestToPolarPoint(const Vector3f &pp) const{
|
|
return segmentPointClosestToCartPoint(pp.toCartesian());
|
|
}
|
|
|
|
float Line6f::segmentDistToCartPoint(const Vector3f &cp) const{
|
|
|
|
Vector3f v = endc - startc; //line segment vector
|
|
Vector3f w1 = cp - startc;
|
|
|
|
if ( w1.innerProduct(v) <= 0) return w1.length();
|
|
|
|
Vector3f w2 = cp - endc;
|
|
|
|
if ( w2.innerProduct(v) >= 0) return w2.length();
|
|
|
|
return v.crossProduct(w1).length() / this->length;
|
|
|
|
}
|
|
|
|
float Line6f::segmentDistToPolarPoint(const Vector3f &pp) const{
|
|
return segmentDistToCartPoint(pp.toCartesian());
|
|
}
|
|
|
|
//source: http://geomalgorithms.com/a07-_distance.html#dist3D_Segment_to_Segment()
|
|
float Line6f::segmentDistToSegment(const Line6f &other) const{
|
|
|
|
Vector3f u = endc - startc;
|
|
Vector3f v = other.endc - other.startc;
|
|
Vector3f w = startc - other.startc;
|
|
float a = u.innerProduct(u); // always >= 0
|
|
float b = u.innerProduct(v);
|
|
float c = v.innerProduct(v); // always >= 0
|
|
float d = u.innerProduct(w);
|
|
float e = v.innerProduct(w);
|
|
float D = a*c - b*b; // always >= 0
|
|
float sc, sN, sD = D; // sc = sN / sD, default sD = D >= 0
|
|
float tc, tN, tD = D; // tc = tN / tD, default tD = D >= 0
|
|
|
|
// compute the line parameters of the two closest points
|
|
if (D < 1e-8f) { // the lines are almost parallel
|
|
sN = 0.0; // force using point start on segment S1
|
|
sD = 1.0; // to prevent possible division by 0.0 later
|
|
tN = e;
|
|
tD = c;
|
|
}
|
|
else { // get the closest points on the infinite lines
|
|
sN = (b*e - c*d);
|
|
tN = (a*e - b*d);
|
|
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
|
|
sN = 0.0;
|
|
tN = e;
|
|
tD = c;
|
|
}
|
|
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
|
|
sN = sD;
|
|
tN = e + b;
|
|
tD = c;
|
|
}
|
|
}
|
|
|
|
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
|
|
tN = 0.0;
|
|
// recompute sc for this edge
|
|
if (-d < 0.0)
|
|
sN = 0.0;
|
|
else if (-d > a)
|
|
sN = sD;
|
|
else {
|
|
sN = -d;
|
|
sD = a;
|
|
}
|
|
}
|
|
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
|
|
tN = tD;
|
|
// recompute sc for this edge
|
|
if ((-d + b) < 0.0)
|
|
sN = 0;
|
|
else if ((-d + b) > a)
|
|
sN = sD;
|
|
else {
|
|
sN = (-d + b);
|
|
sD = a;
|
|
}
|
|
}
|
|
// finally do the division to get sc and tc
|
|
sc = (abs(sN) < 1e-8f ? 0.0 : sN / sD);
|
|
tc = (abs(tN) < 1e-8f ? 0.0 : tN / tD);
|
|
|
|
// get the difference of the two closest points
|
|
Vector3f dP = w + (u * sc) - (v * tc); // = S1(sc) - S2(tc)
|
|
|
|
return dP.length(); // return the closest distance
|
|
|
|
}
|
|
|
|
|
|
Vector3f Line6f::midPointCart() const{
|
|
return (startc+endc)/2;
|
|
}
|
|
|
|
Vector3f Line6f::midPointPolar() const{
|
|
return midPointCart().toPolar();
|
|
}
|
|
|
|
|
|
bool Line6f::operator==(const Line6f& other) const {
|
|
return (startp == other.startp) && (endp == other.endp);
|
|
}
|
|
|
|
const Vector3f &Line6f::get_polar_pt(const int index) const{
|
|
if(index==0) return startp;
|
|
return endp;
|
|
}
|
|
|
|
const Vector3f &Line6f::get_cart_pt(const int index) const{
|
|
if(index==0) return startc;
|
|
return endc;
|
|
}
|
|
|
|
Vector3f Line6f::get_polar_vector() const{
|
|
return endp-startp;
|
|
}
|
|
|
|
Vector3f Line6f::get_cart_vector() const{
|
|
return endc-startc;
|
|
}
|
|
|
|
|
|
|
|
|