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  - http://www.flightgear.org/~curt
   6 //
   7 // This library is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU Library General Public
   9 // License as published by the Free Software Foundation; either
  10 // version 2 of the License, or (at your option) any later version.
  11 //
  12 // This library is distributed in the hope that it will be useful,
  13 // but WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // Library 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., 51 Franklin Street, Fifth Floor, Boston, MA  02110-1301, 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 // calculate the projection, p, of u along the direction of d.
  33 void sgProjection(sgVec3 p, const sgVec3 u, const sgVec3 d){
  34     double denom = sgScalarProductVec3(d,d);
  35     if (denom == 0.) sgCopyVec3(p, u);
  36     else sgScaleVec3(p, d, sgScalarProductVec3(u,d) / denom);
  37 }
  38
  39 // Same thing, except using double precision
  40 void sgProjection(sgdVec3 p, const sgdVec3 u, const sgdVec3 d){
  41     double denom = sgdScalarProductVec3(d,d);
  42     if (denom == 0.) sgdCopyVec3(p, u);
  43     else sgdScaleVec3(p, d, sgdScalarProductVec3(u,d) / denom);
  44 }
  45
  46 // Given a point p, and a line through p0 with direction vector d,
  47 // find the closest point (p1) on the line
  48 void sgClosestPointToLine( sgVec3 p1, const sgVec3 p, const sgVec3 p0,
  49                            const sgVec3 d ) {
  50
  51     sgVec3 u, u1;
  52
  53     // u = p - p0
  54     sgSubVec3(u, p, p0);
  55
  56     // calculate the projection, u1, of u along d.
  57     sgProjection(u1, u, d);
  58
  59     // calculate the point p1 along the line that is closest to p
  60     // p0 = p1 + u1
  61     sgAddVec3(p1, p0, u1);
  62 }
  63
  64
  65 // Given a point p, and a line through p0 with direction vector d,
  66 // find the closest point (p1) on the line
  67 void sgdClosestPointToLine( sgdVec3 p1, const sgdVec3 p, const sgdVec3 p0,
  68                             const sgdVec3 d ) {
  69
  70     sgdVec3 u, u1;
  71
  72     // u = p - p0
  73     sgdSubVec3(u, p, p0);
  74
  75     // calculate the projection, u1, of u along d.
  76     sgProjection(u1, u, d);
  77
  78     // calculate the point p1 along the line that is closest to p
  79     // p0 = p1 + u1
  80     sgdAddVec3(p1, p0, u1);
  81 }
  82
  83
  84 // Given a point p, and a line through p0 with direction vector d,
  85 // find the shortest distance (squared) from the point to the line
  86 double sgClosestPointToLineDistSquared( const sgVec3 p, const sgVec3 p0,
  87                                         const sgVec3 d ) {
  88
  89     sgVec3 u, u1, v;
  90
  91     // u = p - p0
  92     sgSubVec3(u, p, p0);
  93
  94     // calculate the projection, u1, of u along d.
  95     sgProjection(u1, u, d);
  96
  97     // v = u - u1 = vector from closest point on line, p1, to the
  98     // original point, p.
  99     sgSubVec3(v, u, u1);
 100
 101     return ( sgScalarProductVec3(v, v) );
 102 }
 103
 104
 105 // Given a point p, and a line through p0 with direction vector d,
 106 // find the shortest distance (squared) from the point to the line
 107 double sgdClosestPointToLineDistSquared( const sgdVec3 p, const sgdVec3 p0,
 108                                          const sgdVec3 d ) {
 109
 110     sgdVec3 u, u1, v;
 111
 112     // u = p - p0
 113     sgdSubVec3(u, p, p0);
 114
 115     // calculate the projection, u1, of u along d.
 116     sgProjection(u1, u, d);
 117
 118     // v = u - u1 = vector from closest point on line, p1, to the
 119     // original point, p.
 120     sgdSubVec3(v, u, u1);
 121
 122     return ( sgdScalarProductVec3(v, v) );
 123 }
 124
 125
 126 // This is a quicker form of
 127 // sgMakeMatTrans4( sgMat4 sgTrans, sgVec3 trans )
 128 // sgPostMultMat4( sgMat, sgTRANS );
 129 void sgPostMultMat4ByTransMat4( sgMat4 src, const sgVec3 trans )
 130 {
 131     for( int i=0; i<4; i++) {
 132         for( int j=0; j<3; j++ ) {
 133             src[i][j] += (src[i][3] * trans[j]);
 134         }
 135     }
 136 }
 137
 138