// 
//   Script to draw a 3D man (height 180cm) in a 3D view.
//   Copyright (C) 2007 Christian Holm Christensen <cholm@nbi.dk>
//   
//   This  library is free  software; you  can redistribute  it and/or
//   modify  it under  the  terms  of the  GNU  Lesser General  Public
//   License  as published  by  the Free  Software Foundation;  either
//   version  2.1  of the  License,  or  (at  your option)  any  later
//   version.
//
//   This library is distributed in the hope that it will be useful,
//   but WITHOUT ANY WARRANTY; without even the implied warranty of
//   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
//   Lesser General Public License for more details.
//     
//   You should have received a copy of the GNU Lesser General Public
//   License along with this library; if not, write to the Free
//   Software Foundation, Inc., 51 Franklin St, Fifth Floor, Boston,
//   MA 02110-1301 USA
//
#include <TMarker3DBox.h>
#include <TMath.h>
#include <TVector3.h>
#include <TGeoManager.h>
#include <TGeoMatrix.h>
#include <TPad.h>
#include <iostream>

//____________________________________________________________________
class My3DMarker : public TMarker3DBox
{
public:
  My3DMarker(Double_t x,  Double_t y, Double_t z, 
	     Double_t dx, Double_t dy, Double_t dz,
	     Double_t a,  Double_t b,  Double_t c)
    : TMarker3DBox(x,y,z,dx,dy,dz,a,b), 
      fPsi(c)
  {}
  void RotateY(Double_t phi, Double_t* m) const { 
    // Rotate around y
    Double_t sinphi    = TMath::Sin(phi);
    Double_t cosphi    = TMath::Cos(phi);
    Double_t n[] = { cosphi * m[0] + sinphi * m[6], 
		     cosphi * m[1] + sinphi * m[7],
		     cosphi * m[2] + sinphi * m[8],
		     m[3], m[4], m[5], 
		     -sinphi * m[0] + cosphi * m[6], 
		     -sinphi * m[1] + cosphi * m[7],
		     -sinphi * m[2] + cosphi * m[8] };
    for (size_t i = 0; i < 9; i++) m[i] = n[i];
  }
  void RotateX(Double_t psi, Double_t* n) const
  {
    Double_t sinpsi    = TMath::Sin(psi);
    Double_t cospsi    = TMath::Cos(psi);
    Double_t o[] = { n[0], n[1], n[2], 
		     cospsi * n[3] - sinpsi * n[6], 
		     cospsi * n[4] - sinpsi * n[7],
		     cospsi * n[5] - sinpsi * n[8],
		     sinpsi * n[3] + cospsi * n[6], 
		     sinpsi * n[4] + cospsi * n[7],
		     sinpsi * n[5] + cospsi * n[8] };
    for (size_t i = 0; i < 9; i++) n[i] = o[i];
  }
  void RotateZ(Double_t theta, Double_t* o) const
  {
    Double_t sinthe    = TMath::Sin(theta);
    Double_t costhe    = TMath::Cos(theta);
    Double_t q[] = { costhe * o[0] - sinthe * o[3], 
		     costhe * o[1] - sinthe * o[4],
		     costhe * o[2] - sinthe * o[5],
		     sinthe * o[0] + costhe * o[3], 
		     sinthe * o[1] + costhe * o[4],
		     sinthe * o[2] + costhe * o[5],
		     o[6], o[7], o[8] };
    for (size_t i = 0; i < 9; i++) o[i] = q[i];
  }
  void SetPoints(Double_t* p) const
  {
    if (!p) return;

    p[ 0] = -fDx ; p[ 1] = -fDy ; p[ 2] = -fDz;
    p[ 3] = -fDx ; p[ 4] =  fDy ; p[ 5] = -fDz;
    p[ 6] =  fDx ; p[ 7] =  fDy ; p[ 8] = -fDz;
    p[ 9] =  fDx ; p[10] = -fDy ; p[11] = -fDz;
    p[12] = -fDx ; p[13] = -fDy ; p[14] =  fDz;
    p[15] = -fDx ; p[16] =  fDy ; p[17] =  fDz;
    p[18] =  fDx ; p[19] =  fDy ; p[20] =  fDz;
    p[21] =  fDx ; p[22] = -fDy ; p[23] =  fDz;

    Double_t x, y, z;
    const Double_t kPI = TMath::Pi();
    Double_t theta     = fTheta* kPI / 180;
    Double_t phi       = fPhi  * kPI / 180;
    Double_t psi       = fPsi  * kPI / 180;

    Double_t m[] = { 1., 0., 0., 
		     0., 1., 0., 
		     0., 0., 1. };
    RotateZ(theta, m);
    RotateX(psi,   m);
    RotateY(phi,   m);
    
    for (Int_t i = 0; i < 8; i++) {
      x = p[3 * i];
      y = p[3 * i + 1];
      z = p[3 * i + 2];

      p[3 * i + 0] = fX + m[0] * x + m[1] * y + m[2] * z;
      p[3 * i + 1] = fY + m[3] * x + m[4] * y + m[5] * z;
      p[3 * i + 2] = fZ + m[6] * x + m[7] * y + m[8] * z;
    }
  }
protected:
  Double_t fPsi;
};

//____________________________________________________________________
void PlacePart(Double_t x, Double_t y, Double_t z, 
	       const TVector3& c, const TVector3& d, 
	       Int_t    color,  Double_t ang, 
	       Double_t rotz=0, Double_t rotx=0, Double_t roty=0)
{
  Double_t rad = TMath::Pi()*ang/180.;
  TVector3 v(x,y,z);
  v.RotateY(rad);
  TVector3 p(c.X() + v.X(), c.Y() + v.Y(), c.Z() + v.Z());
  My3DMarker* part = new My3DMarker(p.X(),   p.Y(),   p.Z(), 
				    d.X()/2, d.Y()/2 ,d.Z()/2, 
				    rotx,    roty+ang,rotz);
  part->SetLineColor(color);
  part->Draw();
}

//____________________________________________________________________
void DrawMan(Double_t x0, Double_t y0, Double_t z0, 
	     Int_t col, Double_t ang) 
{
  // Draw a 3D man using TMarker3DBox (not in the geometry proper), at
  // (x0,y0,z0) with the color col.  
  Double_t height = 180;
  TVector3 c(x0, y0, z0);
  TVector3 head(20, 25, 20);
  TVector3 neck(15, 8, 15);
  TVector3 upperTorso(40, 30, 28);
  TVector3 lowerTorso(35, 36, 25);
  TVector3 upperArm(8,36,8);
  TVector3 lowerArm(8,30,8);
  TVector3 thigh(15,43,15);
  TVector3 shin(12,33,12);
  TVector3 foot(10,5,25);
  Double_t degrad = TMath::Pi()/180.;
  
  // Head
  Double_t x = 0; // x0;
  Double_t y = height-head.Y()/2;
  Double_t z = 0; // z0;
  PlacePart(x, y, z, c, head, col, ang,  5,  2, 10);

  // Neck
  y -= (head.Y() + neck.Y()) /2;
  PlacePart(x, y, z, c, neck, col, ang);

  // Upper arms 
  Double_t cs = TMath::Cos(30*degrad);
  Double_t sn = TMath::Sin(30*degrad);
  Double_t y1 = y - neck.Y() - cs * upperArm.Y()/2;
  Double_t x1 = (upperTorso.X() + upperArm.X() + sn * upperArm.Y()) / 2;
  PlacePart(-x1, y1, z, c, upperArm, col, ang,  0,  -30);
  PlacePart(+x1, y1, z, c, upperArm, col, ang,  0,  +30);

  
  // Lower arms 
  y1 -= (upperArm.Y() + lowerArm.Y()) / 2;
  PlacePart(-x1, y1, z, c, lowerArm, col, ang,  -30,  30);
  x1 += lowerArm.X();
  PlacePart(+x1, y1, z, c, lowerArm, col, ang,  -30,  0);

  // Torso
  y -= (neck.Y() + upperTorso.Y()) / 2;
  PlacePart(x, y, z+1, c, upperTorso, col, ang);
  y -= (upperTorso.Y() + lowerTorso.Y()) / 2;
  PlacePart(x, y, z, c, lowerTorso, col, ang);

  // Thighs 
  y  -= (lowerTorso.Y() + thigh.Y()) / 2;
  x  =  (lowerTorso.X() - thigh.X()) / 2;
  x1 =  x + TMath::Sin(10 * degrad) * thigh.X();
  PlacePart(-x, y, z, c, thigh, col, ang);
  PlacePart(x1, y, z, c, thigh, col, ang,  -10,   10);

  // Shins 
  y  -= (thigh.Y() + shin.Y()) / 2;
  PlacePart(-x, y, z, c, shin, col, ang);
  x1 += TMath::Sin(10 * degrad) * thigh.X();
  z  += TMath::Sin(10 * degrad) * thigh.Z();
  PlacePart(x1, y, z, c, shin, col, ang, 0, 0, 10);

  // Feet 
  y -= (shin.Y()       + foot.Y())  / 2;
  x =  (lowerTorso.X() - thigh.X()) / 2;
  z =  (foot.Z()       - shin.Z())  / 2;
  PlacePart(-x, y, z, c, foot, col, ang);
  x += TMath::Sin(10 * degrad)*thigh.X();
  z += TMath::Sin(10 * degrad)*thigh.X();
  PlacePart(x1, y, z, c, foot, col, ang, 0, 0, 10);
}
  
//____________________________________________________________________
void
TestMan(Double_t ang=-45)
{
  TGeoManager* geo = new TGeoManager("test", "Test");
  TGeoVolume*  top = geo->MakeBox("top", 0, 200, 200, 200);
  geo->SetTopVolume(top);
  TGeoVolume*  box = geo->MakeBox("box", 0, 10, 100, 5);
  box->SetLineColor(kBlue);
  top->AddNode(box, 1, new TGeoTranslation(-80, 0, 0));
	       
  geo->CloseGeometry();
  top->Draw("ogl");
  
  DrawMan(30, -90, 10, kRed, ang);
  gPad->SetPhi(145);
  gPad->Print("test.png");
}

  

//
// EOF
//