715 |
} |
} |
716 |
|
|
717 |
|
|
718 |
|
template <class Fillet> |
719 |
|
struct Filletoid { |
720 |
|
const CircularNode &node; |
721 |
|
LinearConnectionHalf c; |
722 |
|
Fillet f; |
723 |
|
ZVec dir; |
724 |
|
|
725 |
|
float da; |
726 |
|
vector<float> rtbl; |
727 |
|
|
728 |
|
Filletoid(const CircularNode &node, |
729 |
|
float d, |
730 |
|
float th, |
731 |
|
float a, |
732 |
|
ZVec dir) : |
733 |
|
node(node), |
734 |
|
c(node, 0, d, th, -1, 0), |
735 |
|
f(node, c, a), dir(dir) { |
736 |
|
compute_rtbl(100); |
737 |
|
} |
738 |
|
|
739 |
|
Vec trans(ZVec v) const { |
740 |
|
float x = dir.dot(v); |
741 |
|
float y = (v - x * dir).length(); |
742 |
|
return Vec(x, y); |
743 |
|
} |
744 |
|
|
745 |
|
float rad(ZVec v, bool &success) const { |
746 |
|
Vec t = trans(v); |
747 |
|
if (rtbl.size()) { |
748 |
|
success = true; |
749 |
|
return rad_rtbl(t); |
750 |
|
} |
751 |
|
ZVec pt = f.point(t, success); |
752 |
|
if (success) return pt.length(); |
753 |
|
if (f.infillet(t)) { |
754 |
|
success = true; |
755 |
|
// return distance to the middle of the connection |
756 |
|
return c.d / v.normalized().dot(dir); |
757 |
|
} |
758 |
|
return node.r; |
759 |
|
} |
760 |
|
|
761 |
|
void compute_rtbl(int n) { |
762 |
|
rtbl.resize(n + 1); |
763 |
|
for (int i = 0; i < n; i++) { |
764 |
|
float t = i * (1.0 / n); |
765 |
|
float a = (t * t) * f.tangentAngle; |
766 |
|
bool success; |
767 |
|
float fract; |
768 |
|
ZVec pt = f.point(dirVec(a), success, &fract, .001); |
769 |
|
if (success) |
770 |
|
rtbl[i] = pt.length(); |
771 |
|
else |
772 |
|
rtbl[i] = c.d / cos(a); |
773 |
|
//cout << i << ": " << rtbl[i] << pt << fract << std::endl; |
774 |
|
} |
775 |
|
rtbl[n] = node.r; |
776 |
|
} |
777 |
|
|
778 |
|
float rad_rtbl(Vec v) const { |
779 |
|
int n = rtbl.size() - 1; |
780 |
|
float a = v.atan(); |
781 |
|
float t = sqrt(a / f.tangentAngle); |
782 |
|
int i = (int)(t * n); |
783 |
|
float fract = t * n - i; |
784 |
|
if (i >= n) return rtbl[n]; |
785 |
|
return (1 - fract) * rtbl[i] + fract * rtbl[i + 1]; |
786 |
|
} |
787 |
|
}; |
788 |
|
|
789 |
|
|
790 |
} |
} |
791 |
} |
} |