空间2直线交点算法

// LineInterSect.cpp : Defines the entry point for the console application.
//

#include "stdafx.h"

#include <cmath>
#include <iostream>

using namespace std;

#define ZERO 10e-6

struct Point3D
{
 double x;
 double y;
 double z;
};
class Line
{
public:
 Line(){}
 Line(double a, double b, double c,Point3D pt)
  :vect_a(a),vect_b(b),vect_c(c),lnPoint(pt){}
 Line(const Line& ln)
 {
  vect_a = ln.GetVectI();
  vect_b = ln.GetVectJ();
  vect_c = ln.GetVectK();
  lnPoint = ln.GetPointOnLine();
 }
 Line& operator = (const Line& ln)
 {
  vect_a = ln.GetVectI();
  vect_b = ln.GetVectJ();
  vect_c = ln.GetVectK();
  lnPoint = ln.GetPointOnLine();
  return *this;
 }
 bool operator == (const Line& ln)
 {
  return fabs(ln.GetVectI() - vect_a) < ZERO
   && fabs(ln.GetVectJ() - vect_b) < ZERO
   && fabs(ln.GetVectK() - vect_c) < ZERO
   && fabs(ln.GetPointOnLine().x - lnPoint.x) < ZERO
   && fabs(ln.GetPointOnLine().y - lnPoint.y) < ZERO
   && fabs(ln.GetPointOnLine().z - lnPoint.z) < ZERO;
 }
 double GetVectI() const {return vect_a;}
 double GetVectJ() const {return vect_b;}
 double GetVectK() const {return vect_c;}
 Point3D GetPointOnLine() const {return lnPoint;}
 Point3D GetPointByT(double t)const
 {
  Point3D pt;
  pt.x = vect_a * t + lnPoint.x;
  pt.y = vect_b * t + lnPoint.y;
  pt.z = vect_c * t + lnPoint.z;

  return pt;
 }
 void SetVectI(const double a){vect_a = a;}
 void SetVectJ(const double b){vect_b = b;}
 void SetVectK(const double c){vect_c = c;}
 void SetPointOnLine(const Point3D pt){lnPoint = pt;}
 bool isContainPt(const Point3D pt) const
 {
  if(fabs(vect_a) < ZERO)
  {
   if(fabs(lnPoint.x - pt.x) >= ZERO)
    return false;
   else
   {
    if(fabs(vect_b) < ZERO) // y = bt + Py,here a == 0 && b == 0
    {
     if(fabs(lnPoint.y - pt.y) >= ZERO)
      return false;
     else
     {
      if(fabs(vect_c) < ZERO) // z = ct + Pz (t is a variable)
      {
       return false;
      }
      else
       return true;
     }
    }
    else // y = bt + Py, here a == 0 && b != 0
    {
     if(fabs(vect_c) < ZERO) // here a == 0 && b!= 0 && c == 0
      return true;
     else // here a == 0 && b != 0 && c != 0
     {
      return fabs((lnPoint.y - pt.y) / vect_b
       - (lnPoint.z - pt.z) / vect_c) < ZERO;
     }
    }
   }
  }
  else // here a != 0;
  {
   if(fabs(vect_b) < ZERO && fabs(vect_c) < ZERO)
    return true;
   if(fabs(vect_b) >= ZERO && fabs(vect_c) < ZERO)
   {
    return fabs((lnPoint.y - pt.y) / vect_b
       - (lnPoint.x - pt.x) / vect_a) < ZERO;
   }
   if(fabs(vect_b) < ZERO && fabs(vect_c) >= ZERO)
   {
    return fabs((lnPoint.z - pt.z) / vect_c
       - (lnPoint.x - pt.x) / vect_a) < ZERO;
   }
   if(fabs(vect_b) >= ZERO && fabs(vect_c) >= ZERO)
   {
    return (fabs((lnPoint.y - pt.y) / vect_b
       - (lnPoint.x - pt.x) / vect_a) < ZERO)
       &&( fabs((lnPoint.z - pt.z) / vect_c
       - (lnPoint.x - pt.x) / vect_a) < ZERO);
   }
  }

 }
private:
 double vect_a;
 double vect_b;
 double vect_c;
 Point3D lnPoint;
};

class Plane
{
public:
 Plane(){}
 Plane(double a, double b, double c,Point3D pt)
  :vect_A(a),vect_B(b),vect_C(c),lnPoint(pt){}
 Plane(const Plane& ln)
 {
  vect_A = ln.GetVectI();
  vect_B = ln.GetVectJ();
  vect_C = ln.GetVectK();
  lnPoint = ln.GetPointOnLine();
 }
 Plane& operator = (const Plane& ln)
 {
  vect_A = ln.GetVectI();
  vect_B = ln.GetVectJ();
  vect_C = ln.GetVectK();
  lnPoint = ln.GetPointOnLine();
  return *this;
 }
 bool operator == (const Plane& ln)
 {
  return fabs(ln.GetVectI() - vect_A) < ZERO
   && fabs(ln.GetVectJ() - vect_B) < ZERO
   && fabs(ln.GetVectK() - vect_C) < ZERO
   && fabs(ln.GetPointOnLine().x - lnPoint.x) < ZERO
   && fabs(ln.GetPointOnLine().y - lnPoint.y) < ZERO
   && fabs(ln.GetPointOnLine().z - lnPoint.z) < ZERO;
 }
 double GetVectI() const {return vect_A;}
 double GetVectJ() const {return vect_B;}
 double GetVectK() const {return vect_C;}
 Point3D GetPointOnLine() const {return lnPoint;}

 void SetVectI(const double a){vect_A = a;}
 void SetVectJ(const double b){vect_B = b;}
 void SetVectK(const double c){vect_C = c;}
 void SetPointOnLine(const Point3D pt){lnPoint = pt;}
 bool isContainPt(const Point3D pt) const
 {
  double D = vect_A * (pt.x - lnPoint.x)
      + vect_B * (pt.y - lnPoint.y)
      + vect_C * (pt.z - lnPoint.z);
  
  return fabs(D) < ZERO;
 }
 bool isContainLine(const Line& ln) const
 {
  Point3D pt1 = ln.GetPointByT(0); // param function ,set t = 0
  Point3D pt2 = ln.GetPointByT(1); // param function ,set t = 1
  
  return isContainPt(pt1) && isContainPt(pt2);
 }
private:
 double vect_A;
 double vect_B;
 double vect_C;
 Point3D lnPoint;
};

class commCompute
{
public:

 // this caculates the normal vector of plane defined by two lines
 // this algorithm according to the formula a * b,which likes follows
 //  |i   j   k |
 //  |a1  b1  c1| = (b1 * c2 - b2* c1) * i + (a2 * c1 - a1 * c2) * j + (a1 * b2 - a2 * b1) * k
 //  |a2  b2  c2|
 static Point3D GetPlaneVectByLn(const Line& ln1, const Line& ln2)
 {
  Point3D pt;

  pt.x = ln1.GetVectJ() * ln2.GetVectK() - ln2.GetVectJ() * ln1.GetVectK();
  pt.y = ln2.GetVectI() * ln1.GetVectK() - ln1.GetVectI() * ln2.GetVectK();
  pt.z = ln1.GetVectI() * ln2.GetVectJ() - ln2.GetVectI() * ln1.GetVectJ();
  return pt;
 }

    // assume a1 * b1 * c1 != 0,a2 * b2 * c2 != 0
 static Point3D GetIntersectPt(const Line& ln1, const Line& ln2)
 {
  Point3D pt;
  pt.x = pt.y = pt.z = 0;
  
  if(fabs(ln1.GetVectI()) <ZERO && fabs(ln1.GetVectJ()) < ZERO && fabs(ln1.GetVectK()) < ZERO
   || fabs(ln2.GetVectI()) <ZERO && fabs(ln2.GetVectJ()) < ZERO && fabs(ln2.GetVectK()) < ZERO)
   throw exception("not a line");

  double ln1abc = ln1.GetVectI() + ln1.GetVectJ() + ln1.GetVectK();
  double ln2abc = ln2.GetVectI() + ln2.GetVectJ() + ln2.GetVectK();
  double ln1Pt = ln1.GetPointOnLine().x + ln1.GetPointOnLine().y + ln1.GetPointOnLine().z;
  double ln2Pt = ln2.GetPointOnLine().x + ln2.GetPointOnLine().y + ln2.GetPointOnLine().z;
  double divMatherA = ln2.GetVectI()
   - ln1.GetVectI() * ln2abc / ln1abc;
  double divMatherB = ln2.GetVectJ()
   - ln1.GetVectJ() * ln2abc / ln1abc;
  double divMatherC = ln2.GetVectK()
   - ln1.GetVectK() * ln2abc / ln1abc;
  
  if(fabs(divMatherA) >= ZERO)
  {
   double T2 = (ln1.GetVectI() * (ln2Pt - ln1Pt) / ln1abc
    + ln1.GetPointOnLine().x - ln2.GetPointOnLine().x) / divMatherA;
   pt.x = ln2.GetVectI() * T2 + ln2.GetPointOnLine().x;
   pt.y = ln2.GetVectJ() * T2 + ln2.GetPointOnLine().y;
   pt.z = ln2.GetVectK() * T2 + ln2.GetPointOnLine().z;
   
   return pt;
  }
  if(fabs(divMatherB) >= ZERO)
  {
   double T2 = (ln1.GetVectJ() * (ln2Pt - ln1Pt) / ln1abc
    + ln1.GetPointOnLine().y - ln2.GetPointOnLine().y) / divMatherB;
   pt.x = ln2.GetVectI() * T2 + ln2.GetPointOnLine().x;
   pt.y = ln2.GetVectJ() * T2 + ln2.GetPointOnLine().y;
   pt.z = ln2.GetVectK() * T2 + ln2.GetPointOnLine().z;
   
   return pt;
  }
  if(fabs(divMatherB) >= ZERO)
  {
    double T2 = (ln1.GetVectK() * (ln2Pt - ln1Pt) / ln1abc
    + ln1.GetPointOnLine().z - ln2.GetPointOnLine().z) / divMatherC;
   pt.x = ln2.GetVectI() * T2 + ln2.GetPointOnLine().x;
   pt.y = ln2.GetVectJ() * T2 + ln2.GetPointOnLine().y;
   pt.z = ln2.GetVectK() * T2 + ln2.GetPointOnLine().z;
   
   return pt;
  }
  return pt;
 }
};

int main(int argc, char* argv[])
{
 double a1,b1,c1,a2,b2,c2;
 Point3D pt1, pt2;

 cout << "input the vect<a,b,c> and a point on line" << endl;

 cout << "first line: ";
 cin >> a1 >> b1 >> c1 >> pt1.x >> pt1.y >> pt1.z;
 cout << "sencond line: ";
 cin >> a2 >> b2 >> c2 >> pt2.x >> pt2.y >> pt2.z;

 Line ln1(a1, b1, c1, pt1);
 Line ln2(a2, b2, c2, pt2);

 Point3D normalVector = commCompute::GetPlaneVectByLn(ln1, ln2);

 if(fabs(normalVector.x) < ZERO
  && fabs(normalVector.y) < ZERO
  && fabs(normalVector.z) < ZERO)
 {
  cout << "two lines parallel" << endl;
  return 0;
 }

 Plane p1(normalVector.x, normalVector.y, normalVector.z, pt2);

 if(!p1.isContainLine(ln1) || !p1.isContainLine(ln2))
 {
  cout << "two lines do not intersect" << endl;
 }
 else
 {
  Point3D sectPt = commCompute::GetIntersectPt(ln1, ln2);
  cout << "intersect point is: " << sectPt.x <<", " << sectPt.y <<", " << sectPt.z << endl;

 }

// printf("Hello World!/n");
 return 0;
}