722 |
Fillet f; |
Fillet f; |
723 |
ZVec dir; |
ZVec dir; |
724 |
|
|
725 |
|
float a0; |
726 |
float da; |
float da; |
727 |
|
float sin_da; |
728 |
vector<float> rtbl; |
vector<float> rtbl; |
729 |
|
|
730 |
Filletoid(const CircularNode &node, |
Filletoid(const CircularNode &node, |
731 |
float d, |
float d, |
732 |
float th, |
float th, |
733 |
float a, |
float a, |
734 |
ZVec dir) : |
ZVec dir, |
735 |
|
int tblsize = 0) : |
736 |
node(node), |
node(node), |
737 |
c(node, 0, d, th, -1, 0), |
c(node, 0, d, th, -1, 0), |
738 |
f(node, c, a), dir(dir) { |
f(node, c, a), dir(dir) { |
739 |
compute_rtbl(100); |
if (tblsize) compute_rtbl(tblsize); |
740 |
} |
} |
741 |
|
|
742 |
Vec trans(ZVec v) const { |
Vec trans(ZVec v) const { |
763 |
|
|
764 |
void compute_rtbl(int n) { |
void compute_rtbl(int n) { |
765 |
rtbl.resize(n + 1); |
rtbl.resize(n + 1); |
766 |
for (int i = 0; i < n; i++) { |
|
767 |
|
a0 = atan(0.5 * c.t / c.d); |
768 |
|
|
769 |
|
da = (f.tangentAngle - a0) / n; |
770 |
|
sin_da = sin(da); |
771 |
|
|
772 |
|
for (int i = 1; i < n; i++) { |
773 |
float t = i * (1.0 / n); |
float t = i * (1.0 / n); |
774 |
float a = (t * t) * f.tangentAngle; |
float a = a0 + t * (f.tangentAngle - a0); |
775 |
bool success; |
bool success; |
776 |
float fract; |
float fract; |
777 |
ZVec pt = f.point(dirVec(a), success, &fract, .001); |
ZVec pt = f.point(dirVec(a), success, &fract, .0001); |
778 |
if (success) |
if (!success) { |
779 |
rtbl[i] = pt.length(); |
cout << "ERROR: Point failed for angle " |
780 |
else |
<< a << " (" << i << ")" << std::endl; |
781 |
rtbl[i] = c.d / cos(a); |
} |
782 |
|
rtbl[i] = pt.length(); |
783 |
//cout << i << ": " << rtbl[i] << pt << fract << std::endl; |
//cout << i << ": " << rtbl[i] << pt << fract << std::endl; |
784 |
} |
} |
785 |
|
rtbl[0] = c.d / cos(a0); |
786 |
rtbl[n] = node.r; |
rtbl[n] = node.r; |
787 |
} |
} |
788 |
|
|
789 |
float rad_rtbl(Vec v) const { |
float rad_rtbl(Vec v) const { |
790 |
int n = rtbl.size() - 1; |
int n = rtbl.size() - 1; |
791 |
float a = v.atan(); |
float a = v.atan(); |
792 |
float t = sqrt(a / f.tangentAngle); |
float t = (a - a0) / (f.tangentAngle - a0); |
793 |
int i = (int)(t * n); |
int i = (int)(t * n); |
794 |
float fract = t * n - i; |
if (t < 0) return rtbl[0]; |
795 |
if (i >= n) return rtbl[n]; |
if (i >= n) return rtbl[n]; |
796 |
return (1 - fract) * rtbl[i] + fract * rtbl[i + 1]; |
|
797 |
|
float fract = t * n - i; |
798 |
|
float r0 = rtbl[i]; |
799 |
|
float r1 = rtbl[i + 1]; |
800 |
|
|
801 |
|
return r0 * r1 / (r0 * fract + r1 * (1-fract)); |
802 |
|
return (1 - fract) * r0 + fract * r1; |
803 |
|
return r0 * r1 * sin_da / (r0 * sin(fract * da) + |
804 |
|
r1 * sin((1-fract) * da)) >? r1; |
805 |
} |
} |
806 |
}; |
}; |
807 |
|
|