GJJ
/
PMClassifier
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

closest pt

master
Dtouch 4 months ago
parent
c89448b3ed
commit
baf900b8a2
3 changed files with 155 additions and 61 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 2
      CMakeLists.txt
  2. 200
      include/line.hpp
  3. 14
      include/solid.hpp

2
CMakeLists.txt
View File

@ -1,4 +1,4 @@
cmake_minimum_required(VERSION 3.30)
cmake_minimum_required(VERSION 3.27)
project(PMClassifier)
set(CMAKE_CXX_STANDARD 20)

200
include/line.hpp
View File

@ -1,10 +1,13 @@
#pragma once
#include "vec.hpp"
#include <vector>
#include <iostream>
#include <memory>
#include <cassert>
#include <cmath>
#include <algorithm>
#include <numbers>
// class ILineParam {
// public:
@ -20,15 +23,22 @@
// real t;
// };
struct ClosestDescOnSeg {
real t;
real dis;
};
class ILine {
public:
virtual ~ILine() = default;
virtual Vec3 eval(const double param) const = 0;
virtual Vec3 eval(double param) const = 0;
virtual Vec3 der1(double param) const = 0;
virtual Vec3 tangent(const double param) const = 0;
virtual Vec3 der2(double param) const = 0;
virtual double getClosestParam(const Vec3 &p) const = 0;
virtual ClosestDescOnSeg getClosestParam(const Vec3 &p) const = 0;
};
template<size_t N>
@ -42,96 +52,168 @@ public:
using Point = Vec<3>;
Polyline(const Pt3Array &points, const std::vector<double> &bugles, const Vec3 &normal, bool closed = false)
: points(points), bugles(bugles), closed(closed), normal(normal.normalize()) {
: _points(points), _bugles(bugles), _closed(closed), _normal(normal.normalize()) {
assert(points.size() >= 2);
if (closed) {
assert(points.size() == bugles.size());
} else {
assert(points.size() - 1 == bugles.size());
}
circleCenters.resize(bugles.size());
thetas.resize(bugles.size());
radii.resize(bugles.size());
circularArcs.resize(bugles.size());
for (size_t i = 0; i < bugles.size(); ++i) {
const Point &A = points[i];
const Point &B = points[(i + 1) % points.size()];
Vec3 AB = B - A;
Vec3 ABNorm = AB.normalize();
Vec3 QO = normal.cross(ABNorm) * (abs(bugles[i]) > 1 ? -1 : 1);
float theta = std::atan(bugles[i]) * 4;
float h = AB.length() * 0.5 * std::tan(theta * 0.5);
circleCenters[i] = A + AB * 0.5 + QO * h;
thetas[i] = theta;
radii[i] = (circleCenters[i] - A).length();
Vec3 QONorm = normal.cross(ABNorm) * (abs(bugles[i]) > 1 ? -1 : 1);
real theta = std::atan(bugles[i]) * 4;
circularArcs[i].h = AB.length() * 0.5 * std::tan(theta * 0.5);
circularArcs[i].center = A + AB * 0.5 + QONorm * circularArcs[i].h;
circularArcs[i].theta = theta;
circularArcs[i].radius = (circularArcs[i].center - A).length();
circularArcs[i].u = (A - circularArcs[i].center).normalize();
circularArcs[i].v = ABNorm.cross(circularArcs[i].u);
}
}
private:
Pt3Array points;
std::vector<real> bugles;
Vec3 normal;
bool closed;
Pt3Array circleCenters;
std::vector<real> thetas;
std::vector<real> radii;
Pt3Array _points;
std::vector<real> _bugles;
Vec3 _normal;
bool _closed;
struct CircularArc {
Vec3 center;
real radius;
real theta;
real h;
Vec3 u; // dir of OA
Vec3 v; // u X v = normal
};
std::vector<CircularArc> circularArcs;
public:
Vec3 eval(const real param) const override {
assert(param >= 0 && param <= bugles.size());
Vec3 eval(real param) const override {
assert(param >= 0 && param <= _bugles.size());
int seg = static_cast<int>(param);
real tOnSeg = param - seg;
const auto &A = points[seg];
// const auto &B = points[(seg + 1) % points.size()];
const auto &center = circleCenters[seg];
Vec3 u = (A-center).normalize();
Vec3 v = normal.cross(u);
real phi = tOnSeg * thetas[seg];
const auto &r = radii[seg];
return center + r * (u * std::cos(phi) + v * std::sin(phi));
const auto &arc = circularArcs[seg];
real phi = tOnSeg * arc.theta;
return arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi));
}
Vec3 tangent(const double param) const override {
// TODO:
Vec3 der1(real param) const override {
assert(param >= 0 && param <= _bugles.size());
int seg = static_cast<int>(param);
real tOnSeg = param - seg;
const auto &arc = circularArcs[seg];
real phi = tOnSeg * arc.theta;
return arc.radius * (arc.u * -std::sin(phi) + arc.v * std::cos(phi));
}
double getClosestParam(const Vec3 &p) const override {
// TODO:
return closestParam;
Vec3 der2(real param) const override {
assert(param >= 0 && param <= _bugles.size());
int seg = static_cast<int>(param);
real tOnSeg = param - seg;
const auto &arc = circularArcs[seg];
real phi = tOnSeg * arc.theta;
return -arc.radius * (arc.u * std::cos(phi) + arc.v * std::cos(phi));
}
void addPoint(const Point &point) {
points.push_back(point);
ClosestDescOnSeg getClosestParam(const Vec3 &p) const override {
real closestDis = std::numeric_limits<real>::max();
real closestParam;
for (int i = 0; i < _bugles.size(); ++i) {
const Vec3 &A = _points[i];
const Vec3 &B = _points[(i + 1) % _points.size()];
const auto &arc = circularArcs[i];
real dis2Seg = segPtDist(p, A, B).dis;
if (dis2Seg - arc.h > closestDis) continue;
if ((A - p).length() < closestDis) {
closestDis = (A - p).length();
closestParam = i;
}
if ((B - p).length() < closestDis) {
closestDis = (B - p).length();
closestParam = i + 1;
}
int segInsertedCnt = arc.theta / DISC_ARC_ANGLE;
for (int j = 0; j < segInsertedCnt; ++j) {
real insertParam = i + j * DISC_ARC_ANGLE / arc.theta;
const Vec3 insertPt = eval(insertParam);
real dis2InsertPt = (p - insertPt).length();
if (dis2InsertPt < closestDis) {
closestDis = dis2InsertPt;
closestParam = insertParam;
}
}
}
// TODO: 为了鲁棒和精度，应该在每个可能最近的seg上做newton iteration
int seg = static_cast<int>(closestParam);
// Q = arc.center + arc.radius * (arc.u * std::cos(phi) + arc.v * std::sin(phi))
// d2 = (Q - p)^2
Vec3 q = eval(closestParam);
Vec3 qDer1 = der1(closestParam);
Vec3 qDer2 = der2(closestParam);
real lDer1 = (q - p).dot(qDer1);
int iter = 0;
while (abs(lDer1) > std::numeric_limits<real>::epsilon() * 1e6) {
closestParam -= lDer1 / (qDer1.dot(qDer1) + (q - p).dot(qDer2)); // -der1 / der2
q = eval(closestParam);
qDer1 = der1(closestParam);
qDer2 = der2(closestParam);
lDer1 = (q - p).dot(qDer1);
printf("After iter %d, dL is %lf\n", iter, lDer1);
if (closestParam < seg - std::numeric_limits<real>::epsilon()) {
closestParam = seg;
closestDis = (_points[seg] - p).length();
break;
} else if (closestParam > seg + 1 + std::numeric_limits<real>::epsilon()) {
closestParam = seg + 1;
closestDis = (_points[(seg + 1) % _points.size()] - p).length();
break;
}
}
return {closestParam, closestDis};
}
const Point &getPoint(size_t index) const {
return points[index];
void print() const {
if (_closed) printf("Closed Polyline: \n");
else printf("Open Polyline: \n");
printf("Points: {\n");
for (int i = 0; i < _points.size(); ++i) {
printf("<%lf, %lf, %lf>", _points[i].x, _points[i].y, _points[i].z);
if (i != _points.size() - 1) printf(", ");
}
std::cout << "}" << std::endl;
printf("Bugles: {\n");
for (int i = 0; i < _bugles.size(); ++i) {
printf("%lf", _bugles[i]);
if (i != _bugles.size() - 1) printf(", ");
}
std::cout << "}" << std::endl;
}
size_t size() const {
return points.size();
}
private:
const real DISC_ARC_ANGLE = std::numbers::pi * 0.125;
void clear() {
points.clear();
static ClosestDescOnSeg segPtDist(const Vec3 &p, const Vec3 &A, const Vec3 &B) {
Vec3 AB = B - A;
Vec3 AP = p - A;
real h = std::clamp(AP.dot(AB) / AB.dot(AB), 0., 1.);
return {h, (AP - AB * h).length()};
}
void print() const {
for (const auto &point: points) {
std::cout << "(";
for (size_t i = 0; i < N; ++i) {
std::cout << point[i];
if (i < N - 1) std::cout << ", ";
}
std::cout << ") ";
}
std::cout << std::endl;
}
};
class PolynomialLine : public ILine {
public:
Vec3 eval(double param) const override { return {}; };
Vec3 der1(double param) const override { return {}; };
Vec3 der2(double param) const override { return {}; };
ClosestDescOnSeg getClosestParam(const Vec3 &p) const override { return {}; };
};

14
include/solid.hpp
View File

@ -12,5 +12,17 @@ public:
class IExtrudedSolid : public ISolid {
protected:
virtual Vec3 getClosestPointOnAxis(const Vec3 &p);
Pt3Array _profile;
real rScale;
};
class extrudedSolidPolyline : public IExtrudedSolid {
protected:
Polyline _axis;
};
class extrudedSolidPolynomialLine : public IExtrudedSolid {
protected:
PolynomialLine _axis;
};

Write Preview
Loading…
Cancel
Save