// Module PI : Peida Interactive     PIGraphic3D
// Trace graphiques 3D               R. Ansari  06/98
// LAL (Orsay) / IN2P3-CNRS  DAPNIA/SPP (Saclay) / CEA

#include <stdio.h>
#include <math.h>
#include "pigraph3d.h"

#ifndef M_PI_2
#define M_PI_2 (M_PI/2)
#endif

//++
// Class	PIGraphic3D
// Lib		PI
// include	pigraph3d.h
//
//   	Classe fournissant les services de tracé graphique 
//	en coordonnées utilisateur, et quelques primitives de tracé 3D
//--
//++
// Links	Parent
// PIGraphicUC
//--

//++
// Titre	Constructeurs, méthodes
//--
//++
// PIGraphic3D(PIGraphicGen* g, PIGrCoord x0, PIGrCoord y0, PIGrCoord dx, PIGrCoord dy)
//	Constructeur d'un objet "PIGraph3D" à partir d'un "PIGraphic* g". L'espace
//	de travail du nouvel objet "PIGraphic" correspond au rectangle défini par
//	"(x0,y0)" , "dx,dy" du "PIGraphic" de départ.
// 
// int  kind()
//	Renvoie le type ("= PI_3DGraphics")
//--

/* --Methode-- */
PIGraphic3D::PIGraphic3D(PIGraphicGen* g, PIGrCoord x0, PIGrCoord y0, PIGrCoord dx, PIGrCoord dy)
        : PIGraphicUC(g, x0, y0, dx, dy)
{
  int i,j;
  for(i=0; i<3; i++) 
    for(j=0; j<3; j++)  RE[i][j] = 0.;
  RE[0][0] = RE[1][1] = RE[2][2] = 1.;
  xO = yC = 0.; zO = -10.;
  xC = yC = zC = 0.;
  tO = fO = pO = 0.; 
  daxO = dayO = 0.25;
  lCO = 10.;  dlCO = 0.2;  
  sO = false;
}

/* --Methode-- */
PIGraphic3D::~PIGraphic3D()
{
}

/* --Methode-- */
int PIGraphic3D::kind() 
{
  return PI_3DGraphics; 
}

//++
// Titre	Système de coordonnées 3D et projection
//	Le système de coordonnées 3D est défini par la position de l'observateur (caméra),
//	la direction de visée ou la position du centre du champ, 
//	l'ouverture angulaire de la caméra selon les deux 
//	axes de la projection (X,Y), eventuellement un angle correspondant à la rotation
//	du plan image ("psi"). Les angles d'Euler ("teta", "phi", "psi" ) sont utilisés
//	dans la définition du système de coordonnées et la projection est de type 
//	perspective. 
//
//	Les coordonnées utilisateur définissant le plan image ("PIGraphicUC") vont de 
//	X: "-dax , +dax"   Y : "-day , +day".  
//--

//++
// void  Set3DCS_Obs(double xo, double yo, double zo, double teta, double phi, double psi, -
//                  double dax, double day, double co, double dco=0.2)
//	Définition du système de projection 3D à l'aide de la position de l'observateur 
//	et de la direction de visée.
//|	xo,yo,zo : Position de l'observateur (camera)
//|	teta,phi : Direction de visee
//|	psi : rotation du plan image
//|	dax , day : Ouverture du plan image selon les deux axes = Xmax,Ymax / Z
//|	co : distance au centre du champ
//|	dco : profondeur de champ 
//
// void  Set3DCS(double xc, double yc, double zc, double xo, double yo, double zo, -
//               double dax, double day, double dco=0.2, double psi=0.)
//	Définition du système de projection 3D à partir de la position de l'observateur et 
//	du centre du champ.
//|	xc,yc,zc : Position du centre du champ
//|	xo,yo,zo : Position de l'observateur (camera)
//|	dax , day : Ouverture du plan image selon les deux axes = Xmax,Ymax / Z
//|	dco : profondeur de champ 
//|	psi : rotation du plan image
//--

#if defined(__KCC__) 
#ifndef M_PI
#define M_PI            3.14159265358979323846  /* pi */
#endif
#ifndef M_PI_2
#define M_PI_2          1.57079632679489661923  /* pi/2 */
#endif
#endif

static double deuxpi = 2*M_PI;

/* --Methode-- */
void PIGraphic3D::Set3DCS_Obs(double xo, double yo, double zo, double teta, double phi, double psi, 
                              double dax, double day, double co, double dco)
{
  sO = true;
  xO = xo;  yO = yo;  zO = zo;
  while (teta < 0.)  teta += deuxpi;
  while (teta > deuxpi)  teta -= deuxpi;
  if (teta > M_PI)  { teta = M_PI-teta;  phi = phi+M_PI; }
  while (phi < 0.)  phi += deuxpi;
  while (phi > deuxpi) phi -= deuxpi;
  tO = teta;  fO = phi;  pO = psi;
  if (dax < 0.) dax = -dax;
  if (day < 0.) day = -day;
  daxO = dax;  dayO = day;
  lCO = co;   dlCO = dco;
// Si on ne met  kAxeDirLtoR, le repere semble etre ds le sens inverse -> kAxeDirRtoL  , Reza 16/10/98
  SetUCS(-dax, dax, -day, day, kAxeDirRtoL, kAxeDirDownUp);
  double ct = cos((double)tO);
  double st = sin((double)tO);
// (Teta,Phi) = Direction de visee 
// Les angles d'Euler correspondants sont Teta, Phi+Pi/2
// Le Pi/2 vient que les rotations d'euler se font dans l'ordre
//  Autour de oZ d'angle Phi, autour de oN (nouvel axe X) d'angle Teta
//  Autour du nouvel axe Z (x3) d'angle Psi
  double cf = cos((double)fO+M_PI_2);
  double sf = sin((double)fO+M_PI_2);
  double cp = cos((double)pO);
  double sp = sin((double)pO);
  RE[0][0] = cf*cp-sf*ct*sp;     RE[0][1] = sf*cp+cf*ct*sp;      RE[0][2] = st*sp;
  RE[1][0] = -cf*sp-sf*ct*cp;    RE[1][1] = -sf*sp+cf*ct*cp;     RE[1][2] = st*cp;
  RE[2][0] = sf*st;              RE[2][1] = -cf*st;              RE[2][2] = ct;
  cf = cos((double)fO);
  sf = sin((double)fO);
  zC = ct*lCO+zO;
  xC = st*cf*lCO+xO;
  yC = st*sf*lCO+yO;
}

/* --Methode-- */
void PIGraphic3D::Set3DCS(double xc, double yc, double zc, double xo, double yo, double zo, 
                          double dax, double day, double dco, double psi)
{
  sO = false;
  xO = xo;  yO = yo;  zO = zo;
  xC = xc;  yC = yc;  zC = zc;
  double dx,dy,dz;
// On calcule le vecteur  OC  Observateur -> Centre du champ, ca nous donne donc teta,phi de visee
  dx = xc-xo;  dy = yc-yo;  dz = zc-zo;
  if (dax < 0.) dax = -dax;
  if (day < 0.) day = -day;
  daxO = dax;  dayO = day;  
// Si on ne met  kAxeDirLtoR, le repere semble etre ds le sens inverse -> kAxeDirRtoL  , Reza 16/10/98
  SetUCS(-dax, dax, -day, day, kAxeDirRtoL, kAxeDirDownUp);
  lCO = sqrt(dx*dx+dy*dy+dz*dz);
  if (lCO < 1.e-6) {
    printf("PIGraphic3D::Set3DCS()/Warning : Length(CO) = %g -> CS par defaut \n", lCO);
    int i,j;
    for(i=0; i<3; i++) 
      for(j=0; j<3; j++)  RE[i][j] = 0.;
    RE[0][0] = RE[1][1] = RE[2][2] = 1.;
    xO = yC = 0.; zO = -10.;
    xC = yC = zC = 0.;
    tO = fO = pO = 0.; 
    daxO = dayO = 0.25;
    lCO = 10.;  dlCO = 0.2;      
    return;
  }
  dlCO = dco;
  double lxy = sqrt(dx*dx+dy*dy);
  double ct,st,cf,sf;
  ct = dz/lCO;
  st = sqrt(1-ct*ct);
  pO = psi;
  tO = acos(ct);
  if (lxy < 1.e-6) { fO = 0.;  cf = 0.;  }
  else {  cf = dx/lxy;  fO = acos(cf);  if (dy < 0.)  fO = deuxpi-fO;   }
// (Teta,Phi) = Direction de visee 
// Les angles d'Euler correspondants sont Teta, Phi+Pi/2
// Le Pi/2 vient que les rotations d'euler se font dans l'ordre
//  Autour de oZ d'angle Phi, autour de oN (nouvel axe X) d'angle Teta
//  Autour du nouvel axe Z (x3) d'angle Psi
  cf = cos((double)fO+M_PI_2);
  sf = sin((double)fO+M_PI_2);
  double cp = cos((double)psi);
  double sp = sin((double)psi);
  RE[0][0] = cf*cp-sf*ct*sp;     RE[0][1] = sf*cp+cf*ct*sp;      RE[0][2] = st*sp;
  RE[1][0] = -cf*sp-sf*ct*cp;    RE[1][1] = -sf*sp+cf*ct*cp;     RE[1][2] = st*cp;
  RE[2][0] = sf*st;              RE[2][1] = -cf*st;              RE[2][2] = ct;
}

//++
// bool Get3DCS(double& xc, double& yc, double& zc, double& xo, double& yo, double& zo, -
//             double& teta, double& phi, double& psi, double& dax, double& day, double& co, double& dco);
//	Renvoie les paramètres de définition du système de projection 3D.
//	"Rc= true" si défini par  "Set3DCS_Obs()"
//	
// void  RotateObserver(double teta, double phi, double psi=0., bool rel=true)
//	Positionne l'observateur (la caméra) en maintenant la position  
//	du centre de champ fixe. Si "rel=false" : "teta, phi" determine la nouvelle
//	direction de visée. Si "rel=false" : la direction de visée est tournée
//	de l'angle "teta,phi". 
// void  RotateObject(double teta, double phi, bool rel=true)
//	Repostionne le centre du champ en maintenant la position de l'observateur fixe.
//	Si "rel=false" : "teta, phi" determine la nouvelle direction de visée.
//	Si "rel=false" : la direction de visée est tournée de l'angle "teta,phi". 
// 
// void  ZoomInOut(double fco, double fdax, double fday, bool rel=true)
//	Eloigne ou approche l'observateur et modifie l'ouverture du plan image.
//	si "real=true" : "fco, fdax, fday" sont des facteurs multiplicatifs
//	par rapport aux valeurs actuelles, sinon des valeurs absolues. 
//|	fco : distance observateur - centre du champ
//|	fdax, fday : ouverture du plan image
//--

/* --Methode-- */
bool PIGraphic3D::Get3DCS(double& xc, double& yc, double& zc, double& xo, double& yo, double& zo, 
                          double& teta, double& phi, double& psi, double& dax, double& day, double& co, double& dco)
{
  xc = xC;  yc = yC;  zc = zC;
  xo = xO;  yo = yO;  zo = zO;
  teta = tO;  phi = fO;  psi = pO;
  dax = daxO;   day = dayO;
  co = lCO;     dco = dlCO;
  return(sO);
}


/* --Methode-- */
void PIGraphic3D::RotateObserver(double teta, double phi, double psi, bool rel)
// Positionne l'observateur (la camera) de maniere a ce que la direction de 
// visee soit tO=teta, pO=phi (rel=false) / tO+=teta pO+=phi (rel=true) 
// en conservant le centre de champ fixe  
{
  if (rel) {
    teta += tO;
    phi += fO;
    psi += pO;
    }
  while (teta < 0.)  teta += deuxpi;
  while (teta > deuxpi)  teta -= deuxpi;
  if (teta > M_PI)  { teta = M_PI-teta;  phi = phi+M_PI; }
  double xo,yo,zo; 
  zo = -cos((double)teta)*lCO+zC;
  xo = -sin((double)teta)*cos((double)phi)*lCO+xC;
  yo = -sin((double)teta)*sin((double)phi)*lCO+yC;
  Set3DCS(xC,yC,zC, xo,yo,zo, daxO, dayO, dlCO, psi);
}

/* --Methode-- */
void PIGraphic3D::RotateObject(double teta, double phi, bool rel)
// Positionne l'objet (centre du champ) de maniere a ce que la direction de 
// visee soit tO=teta, pO=phi (rel=false) / tO+=teta pO+=phi (rel=true) 
// en conservant la position de l'observateur fixe.  
{
  if (rel) {
    teta += tO;
    phi += fO;
    }
  while (teta < 0.)  teta += deuxpi;
  while (teta > deuxpi)  teta -= deuxpi;
  if (teta > M_PI)  { teta = M_PI-teta;  phi = phi+M_PI; }
  double xc,yc,zc; 
  zc = cos((double)teta)*lCO+zO;
  xc = sin((double)teta)*cos((double)phi)*lCO+xO;
  yc = sin((double)teta)*sin((double)phi)*lCO+yO;
  Set3DCS(xc,yc,zc, xO,yO,zO, daxO, dayO, dlCO, pO);
}

/* --Methode-- */
void PIGraphic3D::ZoomInOut(double fco, double fdax, double fday, bool rel)
{
  if (fco < 0.001) fco = 1.;
  if (fdax < 0.001) fdax = 1.;
  if (fday < 0.001) fday = 1.;
  double dl;
  if (rel) {
    dl = fco*lCO;  
    fdax *= daxO;
    fday *= dayO;
  }
  else dl = fco; 
  double xo,yo,zo; 
  if (dl < 1.e-5) dl = 1.e-5;
  zo = zC-cos((double)tO)*dl;
  xo = xC-sin((double)tO)*cos((double)fO)*dl;
  yo = yC-sin((double)tO)*sin((double)fO)*dl;
  Set3DCS(xC,yC,zC, xo,yo,zo, fdax, fday, dlCO, pO);
//  Set3DCS_Obs(xo, yo, zo, tO, fO, pO, daxO*fda, dayO*fda, dl, dlCO);
}

/* --Methode-- */
void PIGraphic3D::C3DC2ObsCS(double x, double y, double z, double& xp, double& yp, double& zp)
{
  double xc, yc, zc;
  xc = x-xO;   yc = y-yO;    zc = z-zO;
  xp = RE[0][0]*xc+RE[0][1]*yc+RE[0][2]*zc;
  yp = -RE[1][0]*xc+RE[1][1]*yc+RE[1][2]*zc;  
  zp = RE[2][0]*xc+RE[2][1]*yc+RE[2][2]*zc;
}

/* --Methode-- */
void PIGraphic3D::Proj3DC2GrC(double x, double y, double z, double& xpix, double& ypix)
{
  double xc, yc, zc;
  double xp, yp, zp;
  xc = x-xO;   yc = y-yO;    zc = z-zO;
  xp = RE[0][0]*xc+RE[0][1]*yc+RE[0][2]*zc;   
  yp = RE[1][0]*xc+RE[1][1]*yc+RE[1][2]*zc;
  zp = RE[2][0]*xc+RE[2][1]*yc+RE[2][2]*zc;
  if ((zp>-1.e-6) && (zp < 1.e-6))   // Protection Divide/0
    { xpix = ypix = 0.;   return; }
  xpix = (double)(xOrg + (xp/zp)*xScale);
  ypix = (double)(yOrg + (yp/zp)*yScale);
//  printf("-DBG-Proj3DC2GrC() %g %g %g -> %g %g %g -> %g %g %g -> (%g %g)\n", x,y,z,xc,yc,zc,xp,yp,zp,xp/zp,yp/zp);
//  printf("-DBG- ... (%g %g %g - %g %g) %g %g - %g %g -> %g %g \n", xO,yO,zO, tO,fO, xOrg, yOrg,xScale,yScale ,xpix,ypix);
}

//++
// Titre	Primitives de tracé 3D
//--
//++
// void  DrawString3D(PIGrCoord x0, PIGrCoord y0, PIGrCoord z0, char* s)
//	Tracé de chaîne de caractères à la position "(x0,y0,z0)"
// void  DrawOpaqueString3D(PIGrCoord x0, PIGrCoord y0, PIGrCoord z0, char* s)
//	Tracé de chaîne opaque à la position "(x0,y0,z0)"
// void  DrawLine3D(PIGrCoord x1, PIGrCoord y1, PIGrCoord z1, -
//                  PIGrCoord x2, PIGrCoord y2, PIGrCoord z2)
//	Tracé d'une ligne entre les deux points "(x1,y1,z1)" et "(x2,y2,z2)"
// void  DrawPolygon3D(PIGrCoord *x, PIGrCoord *y, PIGrCoord *z, int n)
//	Tracé de contour de polygone - sommets définis par le tableau 
//	"(x[i],y[i],z[i])" , "i=0..n-1"
// void  DrawFPolygon3D(PIGrCoord *x, PIGrCoord *y, PIGrCoord *z, int n)
//	Tracé de polygone - "(x[i],y[i],z[i])" , "i=0..n-1"
// void  DrawMarker3D(PIGrCoord x0, PIGrCoord y0, PIGrCoord z0)
//	Tracé d'un signe (marker) au point "(x0,y0,z0)"
// void  DrawMarkers3D(PIGrCoord *x, PIGrCoord *y, PIGrCoord *z, int n)
//	Tracé de signes (marker) aux points "(x[i],y[i],z[i])" , "i=0..n-1"
//--

/* --Methode-- */
void PIGraphic3D::DrawString3D(PIGrCoord x0, PIGrCoord y0, PIGrCoord z0, const char* s)
{
  if (!mGrC) return;
  double xf, yf;
  Proj3DC2GrC(x0, y0, z0, xf, yf);
  mGrC->DrawString(xf, yf, s);
  return;
}

/* --Methode-- */
void PIGraphic3D::DrawOpaqueString3D(PIGrCoord x0, PIGrCoord y0, PIGrCoord z0, const char* s)
{
  if (!mGrC) return;
  double xf, yf;
  Proj3DC2GrC(x0, y0, z0, xf, yf);
  mGrC->DrawOpaqueString(xf, yf, s);
  return;
}

/* --Methode-- */
void PIGraphic3D::DrawLine3D(PIGrCoord x1, PIGrCoord y1, PIGrCoord z1, 
                             PIGrCoord x2, PIGrCoord y2, PIGrCoord z2)
{
  if (!mGrC) return;
  double xf1, yf1, xf2, yf2;
  Proj3DC2GrC(x1, y1, z1, xf1, yf1);
  Proj3DC2GrC(x2, y2, z2, xf2, yf2);
//  printf("-DBG-DrawLine3D() : %g %g %g -> %g %g | %g %g %g -> %g %g\n", 
//         (double)x1, (double)y1, (double)z1, xf1, yf1, 
//         (double)x2, (double)y2, (double)z2, xf2, yf2 );
  mGrC->DrawLine(xf1, yf1, xf2, yf2);
}

#define NMXMULTP  30   // Pour multipoint sans new

/* --Methode-- */
void PIGraphic3D::DrawPolygon3D(PIGrCoord *x, PIGrCoord *y, PIGrCoord *z, int n)
{
  PIGrCoord xc[NMXMULTP], yc[NMXMULTP];
  PIGrCoord *pxc, *pyc;
  int i;
  double xf, yf;
 
  if (!mGrC) return;
  if (n <= 0)  return;

  if (n > NMXMULTP) { pxc = new PIGrCoord[n];  pyc = new PIGrCoord[n];  }
  else { pxc = xc;  pyc = yc; }
  for(i=0; i<n; i++) {
    Proj3DC2GrC(x[i], y[i], z[i], xf, yf);
    pxc[i] = xf;  pyc[i] = yf; 
  }
  mGrC->DrawPolygon(pxc, pyc, n, false);
  if (n > NMXMULTP) { delete[] pxc;  delete[] pyc; }
  return;
}


/* --Methode-- */
void PIGraphic3D::DrawFPolygon3D(PIGrCoord *x, PIGrCoord *y, PIGrCoord *z, int n)
{
  PIGrCoord xc[NMXMULTP], yc[NMXMULTP];
  PIGrCoord *pxc, *pyc;
  int i;
  double xf, yf;
 
  if (!mGrC) return;
  if (n <= 0)  return;

  if (n > NMXMULTP) { pxc = new PIGrCoord[n];  pyc = new PIGrCoord[n];  }
  else { pxc = xc;  pyc = yc; }
  for(i=0; i<n; i++) {
    Proj3DC2GrC(x[i], y[i], z[i], xf, yf);
    pxc[i] = xf;  pyc[i] = yf; 
  }
  mGrC->DrawFPolygon(pxc, pyc, n, false);
  if (n > NMXMULTP) { delete[] pxc;  delete[] pyc; }
  return;
}

/* --Methode-- */
void PIGraphic3D::DrawMarker3D(PIGrCoord x0, PIGrCoord y0, PIGrCoord z0)
{
  if (!mGrC) return;
  double xf, yf;
  Proj3DC2GrC(x0, y0, z0, xf, yf);
  mGrC->DrawMarker(xf, yf);
  return;
}

/* --Methode-- */
void PIGraphic3D::DrawMarkers3D(PIGrCoord *x, PIGrCoord *y, PIGrCoord *z, int n)
{
  PIGrCoord xc[NMXMULTP], yc[NMXMULTP];
  PIGrCoord *pxc, *pyc;
  int i;
  double xf, yf;
 
  if (!mGrC) return;
  if (n <= 0)  return;

  if (n > NMXMULTP) { pxc = new PIGrCoord[n];  pyc = new PIGrCoord[n]; }
  else { pxc = xc;  pyc = yc; }
  for(i=0; i<n; i++) {
    Proj3DC2GrC(x[i], y[i], z[i], xf, yf);
    pxc[i] = xf;  pyc[i] = yf; 
  }
  mGrC->DrawMarkers(pxc, pyc, n);
  if (n > NMXMULTP) { delete[] pxc;  delete[] pyc; }
  return;
}


//  Methode pour debugger 
/* --Methode-- */
void PIGraphic3D::PrintCS()
{
if (sO)  printf("PIGraphic3D::PrintCS() : Set3DCS_Obs(...) \n");
else  printf("PIGraphic3D::PrintCS() : Set3DCS(...) \n");
printf("C: %g %g %g ->O: %g %g %g - T,P= %g %g %g lCO= %g %g  |  %g %g - %g %g \n", 
        xC,yC,zC, xO, yO, zO, tO, fO, pO, lCO, dlCO, xOrg, xScale, yOrg, yScale ); 
printf("RE[3][3]=  %g %g %g | %g %g %g | %g %g %g\n", 
        RE[0][0], RE[0][1], RE[0][2], RE[1][0], RE[1][1], RE[1][2], RE[2][0], RE[2][1], RE[2][2]); 
double x,y,z,xp,yp,zp, xpix, ypix;
x = xC, y = yC, z= zC;
C3DC2ObsCS(x, y, z, xp, yp, zp);
Proj3DC2GrC(x, y, z, xpix, ypix);
printf("C -> %g %g %g -> %g %g \n", xp, yp, zp, xpix, ypix); 
x = xC+1, y = yC, z= zC;
C3DC2ObsCS(x, y, z, xp, yp, zp);
Proj3DC2GrC(x, y, z, xpix, ypix);
printf("C+(1,0,0) -> %g %g %g -> %g %g ", xp, yp, zp, xpix, ypix); 
x = xC, y = yC+1, z= zC+1;
C3DC2ObsCS(x, y, z, xp, yp, zp);
Proj3DC2GrC(x, y, z, xpix, ypix);
printf("C+(0,1,1) -> %g %g %g -> %g %g \n", xp, yp, zp, xpix, ypix); 
}
