source: orxonox.OLD/branches/control/src/lib/math/line.cc @ 7134

/*
   orxonox - the future of 3D-vertical-scrollers

   Copyright (C) 2004 orx

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; either version 2, or (at your option)
   any later version.

   ### File Specific:
   main-programmer: Christian Meyer
   co-programmer: Patrick Boenzli : Vector::scale()
                                    Vector::abs()
*/

#define DEBUG_SPECIAL_MODULE DEBUG_MODULE_MATH

#include "line.h"
#include "plane.h"
#ifdef DEBUG
  #include "debug.h"
#else
  #include <stdio.h>
  #define PRINT(x) printf
#endif

using namespace std;

/**
 *  calculate the distance between two lines
 * @param l: the other line
 * @return the distance between the lines
*/
float Line::distance (const Line& l) const
{
  float q, d;
  Vector n = a.cross(l.a);
  q = n.dot(r-l.r);
  d = n.len();
  if( d == 0.0) return 0.0;
  return q/d;
}

/**
 *  calculate the distance between a line and a point
 * @param v: the point
 * @return the distance between the Line and the point
*/
float Line::distancePoint (const Vector& v) const
{
  Vector d = v-r;
  Vector u = a * d.dot( a);
  return (d - u).len();
}

/**
 *  calculate the distance between a line and a point
 * @param v: the point
 * @return the distance between the Line and the point
 */
float Line::distancePoint (const sVec3D& v) const
{
  Vector s(v[0], v[1], v[2]);
  Vector d = s - r;
  Vector u = a * d.dot( a);
  return (d - u).len();
}

/**
 *  calculate the two points of minimal distance of two lines
 * @param l: the other line
 * @return a Vector[2] (!has to be deleted after use!) containing the two points of minimal distance
*/
Vector* Line::footpoints (const Line& l) const
{
  Vector* fp = new Vector[2];
  Plane p = Plane (r + a.cross(l.a), r, r + a);
  fp[1] = p.intersectLine (l);
  p = Plane (fp[1], l.a);
  fp[0] = p.intersectLine (*this);
  return fp;
}

/**
  \brief calculate the length of a line
  \return the lenght of the line
*/
float Line::len() const
{
  return a.len();
}

/**
 *  rotate the line by given rotation
 * @param rot: a rotation
*/
void Line::rotate (const Rotation& rot)
{
  Vector t = a + r;
  t = rotateVector( t, rot);
  r = rotateVector( r, rot),
  a = t - r;
}