// ------------------------------------------------------------------------
  // This program is complementary material for the book:
  //
  // Frank Nielsen
  //
  // Visual Computing: Geometry, Graphics, and Vision
  //
  // ISBN: 1-58450-427-7
  //
  // Charles River Media, Inc.
  //
  //
  // All programs are available at www.charlesriver.com/visualcomputing/
  //
  // You may use this program for ACADEMIC and PERSONAL purposes ONLY. 
  //
  //
  // The use of this program in a commercial product requires EXPLICITLY
  // written permission from the author. The author is NOT responsible or 
  // liable for damage or loss that may be caused by the use of this program. 
  //
  // Copyright (c) 2005. Frank Nielsen. All rights reserved.
  // ------------------------------------------------------------------------
   
  // ------------------------------------------------------------------------
  // File: plucker.cpp
  // 
  // Description: Manipulating Plucker coordinates for line/point incidence,
  // line/line intersection tests, etc. 
  // ------------------------------------------------------------------------
  
  include <stdafx.h>
  include <fstream.h>
  include <math.h>
  
  //using namespace std;
  
  // Inhomogeneous coordinates
  class Point3D{public: double x,y,z;
  
  Point3D()
  {x=y=z=0.0;}
  
  Point3D(double xx, double yy, double zz)
  {x=xx;y=yy;z=zz;}
  
  friend ostream & operator << (ostream & os, const Point3D p)
  {
          os<<"["<<p.x<<","<<p.y<<","<<p.z<<"]";
  return os;
  }
  };
  
  class Plucker{
  public: double c[6];
  
  Plucker::Plucker(Point3D p1, Point3D p2)
  {
  c[0]=p2.x-p1.x;
  c[1]=p2.y-p1.y;
  c[2]=p2.z-p1.z;
  c[3]=p1.y*p2.z-p1.z*p2.y;
  c[4]=p2.x*p1.z-p1.x*p2.z;
  c[5]=p1.x*p2.y-p2.x*p1.y;
  }
  
  friend ostream & operator << (ostream & os, const Plucker l)
  {
          os<<"Plucker Line:["<< l.c[0] <<"," << l.c[1] <<","<<l.c[2]<<","<<l.c[3]<<","<<l.c[4]<<","<<l.c[5]<<"]";
  return os;
  }
  
  }; // Homogeneous coordinates
  
  double PluckerDotProduct(Plucker l1, Plucker l2)
  {
  double s=0;
  
  for(int i=0;i<6;i++)
          s+=l1.c[i]*l2.c[(3+i)%6];
  
  return s;
  }
  
  // The same primitive rewritten differently
  double PluckerLineIntersect(Plucker l1, Plucker l2)
  {
  double s;
  
  s=l1.c[0]*l2.c[3]+l2.c[0]*l1.c[3] +\
  l1.c[1]*l2.c[4]+l2.c[1]*l1.c[4]+\
  l1.c[2]*l2.c[5]+l2.c[2]*l1.c[5];
  
  return s;
  }
  
  double PluckerPointOnLine(Plucker l, Point3D p)
  {
  double rx, ry, rz, rw;
  
  rx=l.c[3]*p.x+l.c[4]*p.y+l.c[5]*p.z;
  ry=-l.c[2]*p.y+l.c[1]*p.z+l.c[3];
  rz=-l.c[2]*p.x+l.c[0]*p.z-l.c[4];
  rw=-l.c[1]*p.x+l.c[0]*p.y+l.c[5];
  
  //cout<<rx<<" "<<ry<< " "<<rz<<" "<<rw<<" "<<endl;
  return fabs(rx)+fabs(ry)+fabs(rz)+fabs(rw);
  }
  
  inline double drand() {return rand()/(double)RAND_MAX;}
  
  int _tmain(int argc, _TCHAR* argv[])
  {
  srand(2555);
  
  Point3D p(drand(),drand(),drand()), q(drand(),drand(),drand()),r, s(drand(),drand(),drand());
  double lambda=0.2;
  
  cout<<"Visual Computing: Geometry, Graphics, and Vision (ISBN:1-58450-427-7)"<<endl;
  cout<<"Demo program\n\n"<<endl;
  
  r.x=p.x*lambda+(1-lambda)*q.x;
  r.y=p.y*lambda+(1-lambda)*q.y;
  r.z=p.z*lambda+(1-lambda)*q.z;
  
  // If you replace r by this code, it is highly likely not to intersect anymore 
  //r=Point3D(drand(),drand(),drand());
  
  Plucker l1(p,q), l2(r,s);
  
  cout<<"Plucker coordinates demo.\n\n"<<endl;
  
  cout<<"Build the line passing through point P="<<p<<" and Q="<<q<<endl;
  cout<<"Plucker coordinates of the line: L1="<<l1<<endl;
  
  cout<<"\n\nBuild the line passing through point R="<<r<<" and S="<<s<<endl;
  cout<<"Plucker coordinates of the line:"<<l2<<endl;
  
  cout<<"Those lines should be contained in a same plane."<<endl;
  cout<<"The dot product of the corresponding Plucker vectors are:"<<PluckerDotProduct(l1,l2)<<endl;
  cout<<"It should be exactly zero if there were no numerical imprecisions"<<endl;
  
  cout<<"\n\nLines L1 and L2 should intersect:"<<PluckerLineIntersect(l1,l2)<<endl;
  cout<<"It should be exactly zero if there were no numerical imprecisions"<<endl;
  
  cout<<"\n\nCheck that point R is on line L1:"<<PluckerPointOnLine(l1,r)<<endl;
  cout<<"It should be exactly zero if there were no numerical imprecisions"<<endl;
  
  cout<<"\n\nPress Return to exit."<<endl;
  char line[256];
  gets(line);
  
          return 0;
  }