cgv
Loading...
Searching...
No Matches
proximity.h
1#pragma once
2
3#include "fvec.h"
4#include <random>
5
6#include "lib_begin.h"
7
8namespace cgv {
9 namespace math {
19 template <typename T>
20 void closest_point_on_sphere_to_point(const fvec<T, 3>& so, T sr, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
21 {
22 n = normalize(p - so);
23 q = sr * n + so;
24 }
35 template <typename T>
36 void closest_point_on_box_to_point(const fvec<T, 3>& bo, const fvec<T, 3>& be, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
37 {
38 int j = 0;
39 for (int i = 0; i < 3; ++i) {
40 q[i] = p[i] - bo[i];
41 if (q[i] < 0) {
42 n[i] = -1;
43 if (q[i] < -0.5f * be[i]) {
44 q[i] = -0.5f * be[i];
45 ++j;
46 }
47 else
48 n[i] = 0;
49 }
50 else {
51 n[i] = 1;
52 if (q[i] > 0.5f * be[i]) {
53 q[i] = 0.5f * be[i];
54 ++j;
55 }
56 else
57 n[i] = 0;
58 }
59 q[i] += bo[i];
60 }
61 if (j > 1)
62 n *= sqrt(T(2));
63 }
75 template <typename T>
76 void closest_point_on_box_to_point(const fvec<T, 3>& bo, const fvec<T, 3>& be, const quaternion<T>& bq, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
77 {
78 fvec<T, 3> p_loc = p-bo;
79 bq.inverse_rotate(p_loc);
80 int j = 0;
81 for (int i = 0; i < 3; ++i) {
82 q[i] = p[i];
83 if (q[i] < 0) {
84 n[i] = -1;
85 if (q[i] < -0.5f * be[i]) {
86 q[i] = -0.5f * be[i];
87 ++j;
88 }
89 else
90 n[i] = 0;
91 }
92 else {
93 n[i] = 1;
94 if (q[i] > 0.5f * be[i]) {
95 q[i] = 0.5f * be[i];
96 ++j;
97 }
98 else
99 n[i] = 0;
100 }
101 }
102 if (j > 1)
103 n *= sqrt(T(2));
104 bq.rotate(n);
105 bq.rotate(q);
106 q += bo;
107 }
118 template <typename T>
119 void closest_point_on_cylinder_to_point(const fvec<T, 3>& co, const fvec<T, 3>& cd, T cr, const fvec<T, 3>& p, fvec<T, 3>& q, fvec<T, 3>& n)
120 {
121 T l = dot(cd, cd);
122 T a = dot(p - co, cd) / l;
123 l = std::sqrt(l);
124 fvec<T, 3> q0 = co + a * cd;
125 T r0 = (p - q0).length();
126 if (a < 0) {
127 if (r0 > cr - a * l)
128 n = (1 / r0) * (p - q0);
129 else
130 n = -cd / l;
131 if (r0 < cr)
132 q = co + p - q0;
133 else
134 q = co + cr / r0 * (p - q0);
135 }
136 else if (a > 1) {
137 if (r0 > cr + (a - 1) * l)
138 n = (1 / r0) * (p - q0);
139 else
140 n = cd / l;
141 if (r0 < cr)
142 q = co + cd + p - q0;
143 else
144 q = co + cd + cr / r0 * (p - q0);
145 }
146 else {
147 q = q0 + (cr / r0) * (p - q0);
148 n = (1 / r0) * (p - q0);
149 }
150 }
161 template <typename T>
162 bool closest_point_on_line_to_line(const fvec<T, 3>& lo, const fvec<T, 3>& ld, const fvec<T, 3>& lo2, const fvec<T, 3>& ld2, fvec<T, 3>& q)
163 {
164 T a = dot(ld, ld);
165 T b = dot(ld, ld2);
166 T e = dot(ld2, ld2);
167
168 T d = a * e - b * b;
169 if (std::abs(d) < 1e-10f) // lines are parallel
170 return false;
171 fvec<T, 3> r = lo - lo2;
172 T c = dot(ld, r);
173 T f = dot(ld2, r);
174 T s = (b * f - c * e) / d;
175 q = lo + s * ld;
176 //T t = (a * f - b * c) / d;
177 return true;
178 }
188 template <typename T, cgv::type::uint32_type N>
189 fvec<T, N> closest_point_on_line_to_point(const fvec<T, N>& lo, const fvec<T, N>& ld, const fvec<T, N>& p)
190 {
191 return lo + (dot(p - lo, ld) / dot(ld, ld)) * ld;
192 }
203 template <typename T>
204 bool closest_point_on_circle_to_point(const fvec<T, 3>& co, const fvec<T, 3>& cn, T cr, const fvec<T, 3>& p, fvec<T, 3>& q)
205 {
206 fvec<T, 3> d = p - co;
207 d -= (dot(d, cn) / dot(cn, cn)) * cn;
208 T l = d.length();
209 if (l < 1e-6f)
210 return false;
211 q = co + (cr / l) * d;
212 return true;
213 }
214 template <typename T>
215 T closest_point_on_line_to_circle_impl(const fvec<T, 3>& lo, const fvec<T, 3>& ld, const fvec<T, 3>& co, const fvec<T, 3>& cn, T cr, fvec<T, 3>& q, int* iter_ptr = 0)
216 {
217 std::default_random_engine e;
218 std::uniform_real_distribution<T> d(-0.01f, 0.01f);
219 q = lo;
220 fvec<T, 3> q0;
221 int iter = 0;
222 T dist = std::numeric_limits<T>::max();
223 do {
224 q0 = q;
225 int cnt = 0;
226 fvec<T, 3> q1 = q;
227 while (!closest_point_on_circle_to_point(co, cn, cr, q, q)) {
228 q += fvec<T, 3>(d(e), d(e), d(e));
229 if (++cnt > 10) {
230 std::cerr << "jittering not working!" << std::endl;
231 return std::numeric_limits<T>::max();
232 }
233 }
234 q1 = q;
235 q = closest_point_on_line_to_point(lo, ld, q);
236 dist = (q - q1).length();
237 if (++iter > 1000) {
238 std::cerr << "no convergence!" << std::endl;
239 if (iter_ptr)
240 *iter_ptr = iter;
241 return std::numeric_limits<T>::max();
242 }
243 } while ((q0 - q).length() > 1e-6f);
244 if (iter_ptr)
245 *iter_ptr = iter;
246 return dist;
247 }
260 template <typename T>
261 T closest_point_on_line_to_circle(const fvec<T, 3>& lo, const fvec<T, 3>& ld, const fvec<T, 3>& co, const fvec<T, 3>& cn, T cr, fvec<T, 3>& q, int* iter_ptr = 0)
262 {
263 T dist = closest_point_on_line_to_circle_impl(lo, ld, co, cn, cr, q);
264 if (dist == std::numeric_limits<T>::max())
265 return dist;
266
267 fvec<T, 3> lo2 = closest_point_on_line_to_point(lo, ld, co);
268 lo2 = 2.0f * lo2 - q;
269 fvec<T, 3> q2;
270 T dist2 = cgv::math::closest_point_on_line_to_circle_impl(lo2, ld, co, cn, cr, q2);
271 if (dist2 == std::numeric_limits<float>::max())
272 return dist2;
273 if (dist < dist2)
274 return dist;
275 q = q2;
276 return dist2;
277 }
278 }
279}
280
281#include <cgv/config/lib_end.h>
cgv
the cgv namespace
Definition print.h:11