| Quellcodebibliothek Statistik Leitseite products/sources/formale Sprachen/Fortran/Fortran77-Compiler/ (Columbo Version 0.7©) Datei vom 9.3.2015 mit Größe 2 MB |
|
Quelle closest_approach.pvs Sprache: PVS |
|||||||||
|
closest_approach[n: posnat]: THEORY
%---------------------------------------------------------------------------- % We compute the closest point of approach (CPA) between two % points that are dynamically moving in a straight line. This is % an important computation in collision detection. For example, % to calculate the time and distance of two planes (represented as % line vectors) when they are at their closest point. % % We use time-parametric equations to represent the paths of the % two planes: % % p(t) = p0 + t*u q(t) = q0 + t*v % % Distance between D(t) is given by: % % D(t) = (u-v)*(u-v) t^2 + 2 * w0 * (u-v) t + w0*w0 % % where w0 = p0 - q0. This is a quadratic equation which has a minimum at -b/2a. % % Minimum separation therefore occurs at: % % t_cpa = -w0*(u-v)/sq(norm(u-v)) % % where w0 = p0 - q0 % % dist(L1,L2,t): MACRO nnreal = dist(loc(L1)(t),loc(L2)(t)) % dist(L1,L2): nnreal = dist(L1,L2,t_cpa(L1,L2)) % % We also introduce the following convenient predicates % % divergent?(p0,q0,u,v): bool = % FORALL (t: posreal): dist(p0,q0) < dist(p0+t*u,q0+t*v) % % dot_prop?(p0,q0,u,v): bool = (p0-q0)*(u-v) >= 0 % % and we prove there equivalence when u /= v. See "dot_prop_divergent". % % See % http://geometryalgorithms.com/Archive/algorithm_0106/algorithm_0106.htm % % for a very nice discussion. % % Author: Ricky Butler NASA Langley Research Center % %---------------------------------------------------------------------------- BEGIN IMPORTING distance[n], reals@quad_minmax t,t0,t1,t2 : VAR real a,b,c : VAR real p0,q0,u,v,w0,wt: VAR Vector[n] sq_dist_lem: LEMMA LET p(t) = p0 + t*u, q(t) = q0 + t*v, w0 = p0 - q0, w(t) = w0 + t*(u-v) IN sq_dist(p(t),q(t)) = w(t)*w(t) % --- distance between two moving points is a quadratic function ---- sq_dist_quad: LEMMA w0 = p0 - q0 AND a = (u-v)*(u-v) AND b = 2*w0*(u-v) AND c = w0*w0 IMPLIES sq_dist(p0+t*u,q0+t*v) = a*sq(t) + b*t + c % ----- p(t) = p0 + t*u q(t) = q0 + t*v ----- dist_eq_vel : LEMMA dist(p0 + t*v, q0 + t*v) = dist(p0,q0) norm_diff_eq_0: LEMMA norm(u-v) = 0 IMPLIES dist(p0+t1*u, q0+t1*v) = dist(p0+t2*u, q0+t2*v) % ----- time of closest point of approach -------- time_closest(p0,q0,u,v): real = IF norm(u-v) = 0 THEN % parallel, eq speed 0 ELSE -((p0-q0)*(u-v))/sq(norm(u-v)) ENDIF time_closest_lem: LEMMA norm(u-v) /= 0 AND a = (u-v)*(u-v) AND b = 2*(p0-q0)*(u-v) IMPLIES time_closest(p0,q0,u,v) = -b/(2*a) % ----- time of closest point of approach between p(t) and q0 time_closest(p0,q0,u): MACRO real = time_closest(p0,q0,u,zero) dist_closest(p0,q0,u,v): MACRO nnreal = LET t_cpa=time_closest(p0,q0,u,v) IN dist(p0+t_cpa*u, q0+t_cpa*v) % ----- distance at closest point is indeed a minimum ------- t_cpa: VAR real d_cpa: VAR nnreal cpa_prep_mono: LEMMA w0 = p0 - q0 AND a = (u-v)*(u-v) AND b = 2*w0*(u-v) AND c = w0*w0 AND t_cpa = time_closest(p0,q0,u,v) AND norm(u-v) /= 0 AND t_cpa <= t1 AND t1 < t2 IMPLIES LET D2 = (LAMBDA t: sq_dist(p0+t*u,q0+t*v)) IN D2(t1) < D2(t2) time_cpa: LEMMA t_cpa = time_closest(p0,q0,u,v) IMPLIES is_minimum?(t_cpa,(LAMBDA t: sq_dist(p0+t*u,q0+t*v))) dist_cpa: LEMMA dist_closest(p0,q0,u,v) <= dist(p0+t*u, q0+t*v) dist_cpa_lt: LEMMA t_cpa = time_closest(p0,q0,u,v) AND norm(u-v) /= 0 AND t_cpa <= t1 AND t1 < t2 IMPLIES LET D2 = (LAMBDA t: sq_dist(p0+t*u,q0+t*v)) IN D2(t1) < D2(t2) dist_min: LEMMA LET p(t) = p0 + t*u, q(t) = q0 + t*v IN t_cpa = time_closest(p0,q0,u,v) IMPLIES dist(p(t_cpa),q(t_cpa)) <= dist(p(t),q(t)) % ------ points diverge from point of closest approach ------ tt: VAR real dist_diverges: LEMMA LET p(t) = p0 + t*u, q(t) = q0 + t*v IN t_cpa = time_closest(p0,q0,u,v) AND t_cpa <= t1 AND t1 < t2 AND norm(u-v) /= 0 IMPLIES dist(p(t1),q(t1)) < dist(p(t2),q(t2)) dist_parallel_lines: LEMMA LET p(t) = p0 + t*u, q(t) = q0 + t*v IN norm(u-v) = 0 IMPLIES dist(p(t1),q(t1)) = dist(p(t2),q(t2)) % ------ Relation between positive dot product and tcpa dot_nneg_tca_npos : LEMMA (p0-q0)*(u-v) >= 0 IMPLIES time_closest(p0,q0,u,v) <= 0 divergent?(p0,q0,u,v): bool = FORALL (t: posreal): dist(p0,q0) < dist(p0+t*u,q0+t*v) dot_prop?(p0,q0,u,v): bool = (p0-q0)*(u-v) >= 0 divergent_u_neq_v : LEMMA divergent?(p0,q0,u,v) IMPLIES u /= v dot_nneg_divergent : THEOREM (p0-q0)*(u-v) >= 0 AND u /= v IMPLIES divergent?(p0,q0,u,v) divergent_dot_nneg : THEOREM divergent?(p0,q0,u,v) IMPLIES (p0-q0)*(u-v) >= 0 divergent_t1_lt_t2 : LEMMA divergent?(p0,q0,u,v) IFF FORALL (t1,t2): 0 <= t1 AND t1 < t2 IMPLIES dist(p0+t1*u,q0+t1*v) < dist(p0+t2*u,q0+t2*v) dot_prop_divergent: THEOREM u /= v IMPLIES (dot_prop?(p0,q0,u,v) IFF divergent?(p0,q0,u,v)) END closest_approach ¤ Dauer der Verarbeitung: 0.15 Sekunden (vorverarbeitet) ¤*© Formatika GbR, Deutschland |
|
2026-03-28