simgear/math/vector.cxx

   1 // vector.cxx -- additional vector routines
   2 //
   3 // Written by Curtis Olson, started December 1997.
   4 //
   5 // Copyright (C) 1997  Curtis L. Olson  - curt@infoplane.com
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #include <math.h>
  25 #include <stdio.h>
  26
  27 // #include <Include/fg_types.h>
  28
  29 #include "vector.hxx"
  30
  31
  32 // Given a point p, and a line through p0 with direction vector d,
  33 // find the shortest distance (squared) from the point to the line
  34 double sgPointLineDistSquared( const sgVec3 p, const sgVec3 p0,
  35                                const sgVec3 d ) {
  36
  37     sgVec3 u, u1, v;
  38     double ud, dd, tmp;
  39
  40     // u = p - p0
  41     sgSubVec3(u, p, p0);
  42
  43     // calculate the projection, u1, of u along d.
  44     // u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
  45     ud = sgScalarProductVec3(u, d);
  46     dd = sgScalarProductVec3(d, d);
  47     tmp = ud / dd;
  48
  49     sgScaleVec3(u1, d, tmp);;
  50
  51     // v = u - u1 = vector from closest point on line, p1, to the
  52     // original point, p.
  53     sgSubVec3(v, u, u1);
  54
  55     return ( sgScalarProductVec3(v, v) );
  56 }
  57
  58
  59 // Given a point p, and a line through p0 with direction vector d,
  60 // find the shortest distance (squared) from the point to the line
  61 double sgdPointLineDistSquared( const sgdVec3 p, const sgdVec3 p0,
  62                                 const sgdVec3 d ) {
  63
  64     sgdVec3 u, u1, v;
  65     double ud, dd, tmp;
  66
  67     // u = p - p0
  68     sgdSubVec3(u, p, p0);
  69
  70     // calculate the projection, u1, of u along d.
  71     // u1 = ( dot_prod(u, d) / dot_prod(d, d) ) * d;
  72     ud = sgdScalarProductVec3(u, d);
  73     dd = sgdScalarProductVec3(d, d);
  74     tmp = ud / dd;
  75
  76     sgdScaleVec3(u1, d, tmp);;
  77
  78     // v = u - u1 = vector from closest point on line, p1, to the
  79     // original point, p.
  80     sgdSubVec3(v, u, u1);
  81
  82     return ( sgdScalarProductVec3(v, v) );
  83 }