| products/sources/formale sprachen/PVS/vectors/ |
|
Quellcode-Bibliothek© Kompilation durch diese Firma[Weder Korrektheit noch Funktionsfähigkeit der Software werden zugesichert.]Datei: closest_approach_2D.pvs Sprache: PVSOriginal von: PVS© |
|
||||||
|
closest_approach_2D: 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 with % % a = (u-v)*(u-v) AND % b = 2*w0*(u-v) AND % c = w0*w0 AND % % which has a minimum at -b/2a. % % Minimum separation 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_2D, reals@quad_minmax t,t0,t1,t2 : VAR real a,b,c : VAR real p0,q0,u,v,w0,wt : VAR Vect2 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) % ----- distance at time of closest point of approach 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)) pos_dot_divergent: THEOREM (p0-q0)*(u-v) > 0 IMPLIES divergent?(p0,q0,u,v) % ------ Intersection of dynamic line and a circle ----- % % See reals@circles_and_lines for similar lemmas s: VAR Vect2 % The first lemma states that if t1 and t2 are the intersection % points with a circle of radius D, then the distance for all times % between t1 and t2 is less than D t1_lt_t2_lt_D : LEMMA FORALL (t,t1,t2,D:real): sqv(v) /= 0 AND t1 < t2 AND sqv(s+t1*v) = D AND sqv(s+t2*v) = D AND t1 < t AND t < t2 IMPLIES sqv(s+t*v) < D % The second lemma is the reciprocal of the previous lemma. It states that % if the distance at time t is less than D, then t is between t1 and % t2. lt_D_t1_lt_t2 : LEMMA FORALL (t,t1,t2,D:real): sqv(v) /= 0 AND t1 < t2 AND sqv(s+t1*v) = D AND sqv(s+t2*v) = D AND sqv(s+t*v) < D IMPLIES t1 < t AND t < t2 % The third lemma states that if there are no intersection points with % a circle of radius D then the distance at any time is greater than D. discr_le_0 : LEMMA FORALL (t,D:real): sqv(v) /= 0 AND discr(sqv(v),2*(s*v),sqv(s) - D) <= 0 IMPLIES sqv(s+t*v) >= D gt_D_t1_lt_t2 : LEMMA FORALL (t,t1,t2,t3,D:real): sqv(v) /= 0 AND t1 <= t AND t < t2 AND t2 < t3 AND sqv(s+t1*v) > D AND sqv(s+t2*v) = D AND sqv(s+t3*v) < D IMPLIES sqv(s+t*v) > D cpa_sqvs_lt: LEMMA 0 <= t1 AND t1 < t2 AND s*v > 0 IMPLIES sqv(s+t1*v) < sqv(s+t2*v) END closest_approach_2D ¤ Dauer der Verarbeitung: 0.18 Sekunden (vorverarbeitet) ¤ |
Haftungshinweis
Die Informationen auf dieser Webseite wurden
nach bestem Wissen sorgfältig zusammengestellt. Es wird jedoch weder Vollständigkeit, noch Richtigkeit,
noch Qualität der bereit gestellten Informationen zugesichert.
Bemerkung:Die farbliche Syntaxdarstellung ist noch experimentell.Bot Zugriff |
