Quellcode-Bibliothek closest_approach_2D.pvs Sprache: 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

quality78%

¤ 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.0.11Bemerkung: (vorverarbeitet) ¤

*Bot Zugriff

Wurzel

Suchen

Beweissystem der NASA

Beweissystem Isabelle

NIST Cobol Testsuite

Cephes Mathematical Library

Wiener Entwicklungsmethode

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.