pt_orient Facet::eval_point(Pt &p)
{
double a = 0, b = 0, c = 0, d = 0;
double distance = 0;
Pt A, B, C;
A = *boundary_edge->getorigin();
B = *boundary_edge->getnext()->getorigin();
C = *boundary_edge->getprev()->getorigin();
a = ((B.Y() - A.Y()) * (C.Z() - A.Z())) -
((C.Y() - A.Y()) * (B.Z() - A.Z()));
b = ((B.Z() - A.Z()) * (C.X() - A.X())) -
((C.Z() - A.Z()) * (B.X() - A.X()));
c = ((B.X() - A.X()) * (C.Y() - A.Y())) -
((C.X() - A.X()) * (B.Y() - A.Y()));
d = -(a * A.X() + b * A.Y() + c * A.Z());
distance = (a* p.X() + b * p.Y() + c * p.Z() + d) /
(sqrt(a*a + b*b + c*c));
if (distance > (0 - 0.0001) && distance < (0 + 0.0001))
{
return E_EQUAL;
}
else if (distance < 0)
{
return E_BELOW;
}
else
return E_ABOVE;
}
double dist(Pt p) const {
if (between(s, e, p)) {
// printf("%lf %lf\n", cross(s, e, p), s.dist(e));
return fabs(cross(s, e, p))/s.dist(e);
}
return min(s.dist(p), e.dist(p));
}
double computeDistances (const Pt& cg, const Pt& p1, const Pt& p2, const Pt& p3, double w1, double w2, double w3)
{
double dist1 = p1.dist(cg);
double dist2 = p2.dist(cg);
double dist3 = p3.dist(cg);
dist1 *= w1;
dist2 *= w2;
dist3 *= w3;
double result = dist1 + dist2 + dist3;
return result;
}
void Reg::ConvexHull() {
cerr << "Calculating Convex Hull Start\n";
vector<Pt> lt = getPoints();
std::sort(lt.begin(), lt.end());
lt[0].angle = -1.0;
for (unsigned int a = 1; a < lt.size(); a++) {
lt[a].calcAngle(lt[0]);
}
std::sort(lt.begin(), lt.end(), sortAngle);
vector<Pt> uh = vector<Pt > ();
uh.push_back(lt[0]);
uh.push_back(lt[1]);
for (int a = 2; a < (int) lt.size();) {
cerr << "List len: " << uh.size() << "\n";
assert(uh.size() >= 2);
Pt point1 = uh[uh.size() - 1];
Pt point2 = uh[uh.size() - 2];
Pt next = lt[a];
cerr << "P1: " << point1.ToString()
<< "P2: " << point2.ToString()
<< "Nx: " << next.ToString()
<< "\n";
if (leftOf(point2, point1, next)) {
uh.push_back(next);
a++;
} else {
uh.pop_back();
}
}
for (unsigned int i = 0; i < uh.size(); i++) {
Pt p1 = uh[i];
Pt p2 = uh[(i + 1) % uh.size()];
Seg s = Seg(p1.x, p1.y, p2.x, p2.y);
convexhull.push_back(s);
}
// convexhull = sortSegs(convexhull);
cerr << "Calculating Convex Hull End\n";
}
PT_ORIENT_T check_coplanar(Pt &p1, Pt &p2, Pt &p3, Pt &check)
{
double a = 0, b = 0, c = 0, d = 0;
double distance = 0;
Pt A, B, C;
A = p1;
B = p2;
C = p3;
a = ((B.Y() - A.Y()) * (C.Z() - A.Z())) -
((C.Y() - A.Y()) * (B.Z() - A.Z()));
b = ((B.Z() - A.Z()) * (C.X() - A.X())) -
((C.Z() - A.Z()) * (B.X() - A.X()));
c = ((B.X() - A.X()) * (C.Y() - A.Y())) -
((C.X() - A.X()) * (B.Y() - A.Y()));
d = -(a * A.X() + b * A.Y() + c * A.Z());
distance = (a* check.X() + b * check.Y() + c * check.Z() + d) /
(sqrt(a*a + b*b + c*c));
if ((distance > 0 - 0.0001) && (distance < 0 + 0.0001))
{
return E_EQUAL;
}
else if (distance < 0)
{
return E_BELOW;
}
else
return E_ABOVE;
}
/****************************** Before function ********************************/
bool Pt::Before(Pt p1)
{
return ((m_x<p1.getX() || m_x==getX()) && (m_y<p1.getY() || m_y==p1.getY()));
}
bool Pt::CloserTo(const Pt& p1,const Pt& pref) const
{
return Euclide(pref) < p1.Euclide(pref);
}
double getAngle(Pt va, Pt vb) { // segment, not vector
return acos(dot(va, vb) / va.length() / vb.length());
}
bool operator()(const Seg &x, const Seg &y) {
Pt v1 = getIntersect(ray_s, ray_e, x.s, x.e);
Pt v2 = getIntersect(ray_s, ray_e, y.s, y.e);
return cmpZero(ray_s.dist2(v1) - ray_s.dist2(v2)) < 0;
}
bool check_collinear(Pt &a, Pt &b, Pt &c)
{
double ratio1 = 0, ratio2 = 0, ratio3 = 0;
ratio1 = (b.X() - a.X()) / (c.X() - a.X());
ratio2 = (b.Y() - a.Y()) / (c.Y() - a.Y());
ratio3 = (b.Z() - a.Z()) / (c.Z() - a.Z());
if ((ratio1 == ratio2) && (ratio3 == ratio2))
return true;
return false;
}
/* \brief Project a point into the world.
*/
inline void projectPoint(Float u, Float v, Float depth, Pt<4> K, Pt<3> &ret){
Float x = (u - K[2]) / K[0]*depth,
y = (v - K[3]) / K[1]*depth,
z = depth;
ret.init(x, y, z);
}