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 }
187 template <typename T>
188 fvec<T, 3> closest_point_on_line_to_point(const fvec<T, 3>& lo, const fvec<T, 3>& ld, const fvec<T, 3>& p)
189 {
190 return lo + (dot(p - lo, ld) / dot(ld, ld)) * ld;
191 }
202 template <typename T>
203 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)
204 {
205 fvec<T, 3> d = p - co;
206 d -= (dot(d, cn) / dot(cn, cn)) * cn;
207 T l = d.length();
208 if (l < 1e-6f)
209 return false;
210 q = co + (cr / l) * d;
211 return true;
212 }
213 template <typename T>
214 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)
215 {
216 std::default_random_engine e;
217 std::uniform_real_distribution<T> d(-0.01f, 0.01f);
218 q = lo;
219 fvec<T, 3> q0;
220 int iter = 0;
221 T dist = std::numeric_limits<T>::max();
222 do {
223 q0 = q;
224 int cnt = 0;
225 fvec<T, 3> q1 = q;
226 while (!closest_point_on_circle_to_point(co, cn, cr, q, q)) {
227 q += fvec<T, 3>(d(e), d(e), d(e));
228 if (++cnt > 10) {
229 std::cerr << "jittering not working!" << std::endl;
230 return std::numeric_limits<T>::max();
231 }
232 }
233 q1 = q;
234 q = closest_point_on_line_to_point(lo, ld, q);
235 dist = (q - q1).length();
236 if (++iter > 1000) {
237 std::cerr << "no convergence!" << std::endl;
238 if (iter_ptr)
239 *iter_ptr = iter;
240 return std::numeric_limits<T>::max();
241 }
242 } while ((q0 - q).length() > 1e-6f);
243 if (iter_ptr)
244 *iter_ptr = iter;
245 return dist;
246 }
259 template <typename T>
260 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)
261 {
262 T dist = closest_point_on_line_to_circle_impl(lo, ld, co, cn, cr, q);
263 if (dist == std::numeric_limits<T>::max())
264 return dist;
265
266 fvec<T, 3> lo2 = closest_point_on_line_to_point(lo, ld, co);
267 lo2 = 2.0f * lo2 - q;
268 fvec<T, 3> q2;
269 T dist2 = cgv::math::closest_point_on_line_to_circle_impl(lo2, ld, co, cn, cr, q2);
270 if (dist2 == std::numeric_limits<float>::max())
271 return dist2;
272 if (dist < dist2)
273 return dist;
274 q = q2;
275 return dist2;
276 }
277 }
278}
279
280#include <cgv/config/lib_end.h>
cgv
the cgv namespace
Definition print.h:11