// gPlane.cpp

#include "stdafx.h"
#include <float.h> // for DBL_MIN
#include "gPlane.h"

gPlane::gPlane() : FlatLand(true) {}
gPlane::~gPlane() {}
gPlane::gPlane(const double* Matrix, bool IsInverse/*=false*/) : FlatLand(true) {IsInverse ? SetFromInverse(Matrix) : SetFromMatrix(Matrix);} // Axis-defined Plane
gPlane::gPlane(const g3Vector& zAxis, const g3Point& Origin/*=g3Point(0,0,0)*/, const g3Vector& dx/*=g3Vector(0,0,0)*/) {Set(zAxis,Origin,dx);} // Axis-defined Plane
gPlane::gPlane(const g3PV& PV, const g3Vector& xAxis/*=g3Vector(0,0,0)*/) {Set(PV,xAxis);}

void gPlane::Set(const g3PV& PV, const g3Vector& xAxis/*=g3Vector(0,0,0)*/) {Set(PV.V,PV.P,xAxis);} // Set from Origin, Normal(zAxis), and xAxis. The vectors don't need to be normalised.

void gPlane::Set(const g3Vector& zAxis, const g3Point& Origin/*=g3Point(0,0,0)*/, const g3Vector& xAxis/*=g3Vector(0,0,0)*/) { // Set from Origin, Normal(zAxis), and xAxis. The vectors don't need to be normalised.
  g3Vector dx(xAxis.Normalised()),dy,dz(zAxis.Normalised());
  FlatLand=((dz.z==1)&&((dx.x==1)||(xAxis.IsNull()))); // FlatLand: no transformations required but set matrices anyway in case NoFlatLand() is called (optimising this will break things)!
  if(dx.IsNull()) dz.GetAxes(dx,dy);
  else dx=(dy=dz.Cross(dx)).Cross(dz); // Make sure dx is orthogonal to PV.V even if xAxis is not.
  SetMatrices(Origin,dx,dy,dz);
}

void gPlane::SetToIdentity() {
  ZeroMemory(Matrix , sizeof Matrix );
  ZeroMemory(Inverse, sizeof Inverse);
  Matrix[0]=Inverse[0]=Matrix[5]=Inverse[5]=Matrix[10]=Inverse[10]=1;
  FlatLand=true;
}

bool gPlane::SetFromMatrix(const double* M/*=0*/) {
  if(M==0) {
    SetToIdentity();
    return false;
  }
  memcpy(Matrix, M, sizeof(Matrix));
  g3Vector dx(Matrix[0], Matrix[4], Matrix[ 8]);
  g3Vector dy(Matrix[1], Matrix[5], Matrix[ 9]);
  g3Vector dz(Matrix[2], Matrix[6], Matrix[10]);
  g3Point   p(Matrix[3], Matrix[7], Matrix[11]);
  memcpy(&Inverse[0], &dx, sizeof(g3Vector));
  memcpy(&Inverse[4], &dy, sizeof(g3Vector));
  memcpy(&Inverse[8], &dz, sizeof(g3Vector));
  Inverse[ 3]=-dx.Dot(p);
  Inverse[ 7]=-dy.Dot(p);
  Inverse[11]=-dz.Dot(p);
  SetFlatLand();
  return true;
}

bool gPlane::SetFromInverse(const double* I/*=0*/) {
  if(I==0) {
    SetToIdentity();
    return false;
  }
  memcpy(Inverse, I, sizeof(Inverse));
  Matrix[0]=I[0]; Matrix[1]=I[4]; Matrix[ 2]=I[ 8];
  Matrix[4]=I[1]; Matrix[5]=I[5]; Matrix[ 6]=I[ 9];
  Matrix[8]=I[2]; Matrix[9]=I[6]; Matrix[10]=I[10];
  double Determinant=I[0]*(I[5]*I[10]-I[9]*I[6]) + I[4]*(I[9]*I[2]-I[1]*I[10]) + I[8]*(I[1]*I[6]-I[5]*I[2]);
  bool OK=(Determinant!=0);
  if(!OK) Determinant=2*DBL_MIN; // Avoid Divide by zero
  Matrix[ 3]=(I[1]*(I[7]*I[10]-I[6]*I[11]) + I[5]*(I[2]*I[11]-I[3]*I[10]) + I[9]*(I[6]*I[3]-I[2]*I[7]))/Determinant;
  Matrix[ 7]=(I[0]*(I[6]*I[11]-I[7]*I[10]) + I[4]*(I[3]*I[10]-I[2]*I[11]) + I[8]*(I[2]*I[7]-I[6]*I[3]))/Determinant;
  Matrix[11]=(I[0]*(I[7]*I[ 9]-I[5]*I[11]) + I[4]*(I[1]*I[11]-I[3]*I[ 9]) + I[8]*(I[5]*I[3]-I[1]*I[7]))/Determinant;
  SetFlatLand();
  return OK;
}

void gPlane::GetArray(double* dst, bool WantInverse/*=false*/) const {memcpy(dst, WantInverse ? Inverse : Matrix, sizeof(Matrix));}
void gPlane::MakeFlatLand(bool flatLand/*=true*/) {FlatLand=flatLand;} // FlatLand: no transformations required
bool gPlane::SetFlatLand () {return (FlatLand=((Matrix[10]==1)&&(Matrix[0]==1)));} // FlatLand: no transformations required
void gPlane::NoFlatLand  () {FlatLand=false;}
bool gPlane::IsFlatLand  ()                                 const {return FlatLand;}
bool gPlane::IsHorizontal(const g3Vector& v)                const {return FlatLand ? v.IsHorizontal()   : ((Inverse[8]*v.x+Inverse[9]*v.y+Inverse[10]*v.z)==0);} // Dot Product of v with z axis = 0 if perp to z axis.
bool gPlane::IsUp        (const g3Vector& v)                const {return FlatLand ? v.z== 1            : ((Inverse[8]== v.x) && (Inverse[9]== v.y) && (Inverse[10]== v.z));}
bool gPlane::IsDown      (const g3Vector& v)                const {return FlatLand ? v.z==-1            : ((Inverse[8]==-v.x) && (Inverse[9]==-v.y) && (Inverse[10]==-v.z));}
bool gPlane::IsVertical  (const g3Vector& v)                const {return FlatLand ? v.IsVertical()     : (IsUp(v) || IsDown(v));}
g3PV     gPlane::GetPV(bool NormalisePlaneOrigin/*=false*/) const {return FlatLand ? g3PV(0,0,0, 0,0,1) : g3PV(Matrix[3],Matrix[7],Matrix[11], Inverse[8],Inverse[9],Inverse[10], NormalisePlaneOrigin);}
g3Vector gPlane::GetXAxis()                                 const {return FlatLand ? g3Vector(1,0,0)    : g3Vector(&Inverse[0]);} // May not be a unit Vector!
g3Vector gPlane::GetYAxis()                                 const {return FlatLand ? g3Vector(0,1,0)    : g3Vector(&Inverse[4]);} // May not be a unit Vector!
g3Vector gPlane::GetZAxis()                                 const {return FlatLand ? g3Vector(0,0,1)    : g3Vector(&Inverse[8]);} // May not be a unit Vector!
g3Point  gPlane::GetOrigin()                                const {return FlatLand ? g3Point (0,0,0)    : g3Point(Matrix[3],Matrix[7],Matrix[11]);}
gCoord   gPlane::GetDisplacement()                          const {return Inverse[11];} // from Origin (negative if in lower global hemisphere)
gCoord   gPlane::GetDistanceFromPlane(const g3Point& P)     const {return FlatLand ? P.z : (P.x-Matrix[3])*Inverse[8] + (P.y-Matrix[7])*Inverse[9] + (P.z-Matrix[11])*Inverse[10];} // Optimised from: (P-GetOrigin()).Dot(GetZAxis());}
g3Point  gPlane::GetClosestPointTo   (const g3Point& P)     const {return FlatLand ? P   :  P-GetZAxis()*(Inverse[11] + P.x*Inverse[8] + P.y*Inverse[9] + P.z*Inverse[10]);} // Optimised from: P-dz*(GetDisplacement()+dz.Dot(P)));
g3Vector gPlane::TransformToLocal    (const g3Vector& v)    const {return g3Vector(v.Dot(Inverse), v.Dot(&Inverse[4]), v.Dot(&Inverse[8]));}
g3Vector gPlane::TransformToGlobal   (const g3Vector& v)    const {return g3Vector(v.Dot(Matrix ), v.Dot(&Matrix [4]), v.Dot(&Matrix [8]));}
g3Point  gPlane::TransformToLocal    (const g3Point& p)     const {return Multiply(p,Inverse);}
g3Point  gPlane::TransformToGlobal   (const g3Point& p)     const {return Multiply(p,Matrix );}

#ifdef g2h // Some applications may not want to bother with the 2D classes
  g2Vector gPlane::TransformToPlane (const g3Vector& v)  const {return g2Vector(v.Dot(Inverse), v.Dot(&Inverse[4]));}
  g3Vector gPlane::TransformToGlobal(const g2Vector& v)  const {return g3Vector(v.Dot(Matrix ), v.Dot(&Matrix [4]), v.Dot(&Matrix [8]));}
  g2Point  gPlane::TransformToPlane (const g3Point& p)   const {return Multiply2(p,Inverse);}
  g3Point  gPlane::TransformToGlobal(const g2Point& p)   const {return Multiply(p,Matrix );}
  g3D gPlane::Multiply(const g2D& d, const double m[12]) const {
    return FlatLand ? g3D(d) : g3D(
      m[0]*d.x+m[1]*d.y+m[ 3],
      m[4]*d.x+m[5]*d.y+m[ 7],
      m[8]*d.x+m[9]*d.y+m[11]
    );
  }
  g2D gPlane::Multiply2(const g3D& d, const double m[12]) const {
    return FlatLand ? d : g2D(
      m[0]*d.x+m[1]*d.y+m[ 2]*d.z+m[ 3],
      m[4]*d.x+m[5]*d.y+m[ 6]*d.z+m[ 7]
    );
  }
#endif

g3D gPlane::Multiply(const g3D& d, const double m[12]) const {
  return FlatLand ? d : g3D(
    m[0]*d.x+m[1]*d.y+m[ 2]*d.z+m[ 3],
    m[4]*d.x+m[5]*d.y+m[ 6]*d.z+m[ 7],
    m[8]*d.x+m[9]*d.y+m[10]*d.z+m[11]
  );
}

// Note that the system decides to stop FlatLand optimisation as soon as you Set using any of the following methods:
// Call SetFlatLand(); to get the system to determine if FlatLand can be set for the plane after the Sets are complete.
void gPlane::SetMatrices(const g3Point& P, const g3Vector& dx, const g3Vector& dy, const g3Vector& dz) {
  //TRACE("  Plane zAxis=(%g,%g,%g) Origin=(%g,%g,%g)\r\n", dz.x.Report(), dz.y.Report(), dz.z.Report(), p.x.Report(), p.y.Report(), p.z.Report());
  Matrix[ 3]=P.x;
  Matrix[ 7]=P.y;
  Matrix[11]=P.z;
  SetXAxis(dx); // Axes depend on Origin being set
  SetYAxis(dy);
  SetZAxis(dz);
}
void gPlane::SetOrigin(const g3Point& P) { // Must have valid Axes to set Inverse!
  FlatLand=false;
  Matrix[ 3]=P.x; Inverse[ 3]=-GetXAxis().Dot(P);
  Matrix[ 7]=P.y; Inverse[ 7]=-GetYAxis().Dot(P);
  Matrix[11]=P.z; Inverse[11]=-GetZAxis().Dot(P);
}
void gPlane::SetXAxis(const g3Vector& dx) { // Must have a valid Origin!
  FlatLand=false;
  Inverse[0]=Matrix[0]=dx.x; Inverse[3]=-dx.Dot(GetOrigin());
  Inverse[1]=Matrix[4]=dx.y;
  Inverse[2]=Matrix[8]=dx.z;
}
void gPlane::SetYAxis(const g3Vector& dy) { // Must have a valid Origin!
  FlatLand=false;
  Inverse[4]=Matrix[1]=dy.x;
  Inverse[5]=Matrix[5]=dy.y; Inverse[7]=-dy.Dot(GetOrigin());
  Inverse[6]=Matrix[9]=dy.z;
}
void gPlane::SetZAxis(const g3Vector& dz) { // Must have a valid Origin!
  FlatLand=false;
  Inverse[ 8]=Matrix[ 2]=dz.x;
  Inverse[ 9]=Matrix[ 6]=dz.y;
  Inverse[10]=Matrix[10]=dz.z; Inverse[11]=-dz.Dot(GetOrigin());
}