source: Sophya/trunk/SophyaLib/Samba/vector3d.cc@ 738

#include "machdefs.h"
#include <math.h>
#include "pexceptions.h"
#include "vector3d.h"
#include "utilgeom.h"
//++ 
// Class        Vector3d
//
// include      vector3d.h utilgeom.h  longlat.h math.h
//
//
//    3-D geometry. 
//    All computations are made with angles in radians and with spherical
//    coordinates theta, phi.
//
//    Concerning Euler's angles, the reference is :
// 
//    "Classical Mechanics" 2nd edition, H. Goldstein, Addison Wesley
//--
//++
// Titre        Constructors
//--
//++
Vector3d::Vector3d()
//
//-- 
{
  Setxyz(1.,0.,0.);
}
//++
Vector3d::Vector3d(double x, double y, double z) 
//
//-- 
{
  _x=x;
  _y=y;
  _z=z;
  xyz2ThetaPhi();
}
//++
Vector3d::Vector3d(double theta, double phi) 
//
//-- 
{
  _theta=mod(theta,M_PI); // dans [0;pi]
  _phi=mod(phi,pi2); // dans [0;2pi]
  ThetaPhi2xyz();
}
//++
Vector3d::Vector3d(const LongLat& ll)
//
//-- 
{
  _theta=ll.Theta(); // dans [0;pi]
  _phi=ll.Phi(); // dans [0;2pi]
  ThetaPhi2xyz();
}
//++
Vector3d::Vector3d(const Vector3d& v) 
//
//-- 
{
  _x=v._x;
  _y=v._y;
  _z=v._z;
  _theta=v._theta;
  _phi=v._phi;
}
//++
// Titre        Public methods
//--
//++
void Vector3d::SetThetaPhi(double theta, double phi)
//
//-- 
{
  _theta=mod(theta,M_PI);
  _phi=mod(phi,pi2);
  ThetaPhi2xyz();
}
//++
void Vector3d::Setxyz(double x, double y, double z) 
//
//-- 
{
  _x=x;
  _y=y;
  _z=z;
  xyz2ThetaPhi();
}
//++
void Vector3d::ThetaPhi2xyz()
//
//-- 
{
  _x=sin(_theta)*cos(_phi);
  _y=sin(_theta)*sin(_phi);
  _z=cos(_theta);
}
//++
void Vector3d::xyz2ThetaPhi()
//
//-- 
{
  double norm=this->Norm();
  if( norm != 0. ) 
    {
      _theta=acos(_z/norm); // dans [0,Pi]
      if( mod(_theta,M_PI) == 0. ) _phi=0.; // on est sur +-Oz, le vecteur z est en phi=0
      //      else _phi=acos(_x/sin(_theta)/norm)+M_PI*(_y<0);
      else _phi=scangle(_y/sin(_theta)/norm,_x/sin(_theta)/norm);
    }
  else // vecteur nul
    {
      _theta=0.;
      _phi=0.;
    }
}
//++
Vector3d& Vector3d::Normalize() 
//
//-- 
{
  double norm=this->Norm();
  if( norm != 0. )  (*this)/=norm;
  else cerr << "Division par zero" << endl;
  return *this;
} 
//++
double Vector3d::Norm() const 
//
//-- 
{
  return sqrt(_x*_x+_y*_y+_z*_z);
}
//++
double Vector3d::Psc(const Vector3d& v) const 
//
//    dot product
//-- 
{
  return _x*v._x+_y*v._y+_z*v._z;
}
//++
double Vector3d::SepAngle(const Vector3d& v) const 
//
//    angular gap between 2 vectors in [0,Pi]
//-- 
{
  double n1=this->Norm();
  double n2=v.Norm();
  double ret;
  if( n1!=0. && n2!=0. ) ret=acos((this->Psc(v))/n1/n2);
  else 
    {
      cerr << "Division par zero" << endl;
      ret=0.;
    }
  return ret;
}
//++
Vector3d Vector3d::Vect(const Vector3d& v) const 
//
//    vector product
//-- 
{
  double xo=_y*v._z-_z*v._y;
  double yo=_z*v._x-_x*v._z;
  double zo=_x*v._y-_y*v._x;
  return Vector3d(xo,yo,zo);
}
//++
Vector3d Vector3d::VperpPhi() const 
//
//    perpendicular vector, with equal phi
//-- 
{
  double theta;
  if( _theta != pi_over_2 ) theta=_theta+(0.5-(_theta>pi_over_2))*M_PI; // on tourne theta de +-pi/2
  else theta=0.;
  return Vector3d(theta,_phi);
}
//++
Vector3d Vector3d::VperpTheta() const 
//
//    perpendicular vector with equal theta
//-- 
{ 
  throw PError("Vector3d::VperpTheta() - Logic Error DY/Reza 20/02/2000"); 
  // Bug ??? (D. Yvon, Fevrier 2000)
  //  double phi=mod(_phi+pi_over_2,pi2); // on tourne phi de pi/2
  //  return Vector3d(_theta,phi);
}

Vector3d Vector3d::EPhi() const 
{
  Vector3d temp;
  if ( fabs(_z) == 1. ) // si on est en +- Oz
    {
      temp=Vector3d(1.,0.,0.);
    }
  else 
    {
      Vector3d k(0,0,-1);
      temp=this->Vect(k);
      temp.Normalize();
    }
  return temp;
}
//++
Vector3d Vector3d::ETheta() const 
//
//-- 
{
  Vector3d temp=this->Vect(EPhi());
  temp.Normalize();
  return temp;
}

//++
Vector3d Vector3d::Euler(double phi, double theta, double psi) const 
//
//    Euler's rotations
//-- 
{
  double cpsi=cos(psi);
  double ctheta=cos(theta);
  double cphi=cos(phi);
  double spsi=sin(psi);
  double stheta=sin(theta);
  double sphi=sin(phi);
  double xnew=(cpsi*cphi-ctheta*sphi*spsi)*_x
    +(cpsi*sphi+ctheta*cphi*spsi)*_y
    +spsi*stheta*_z;
  double ynew=(-spsi*cphi-ctheta*sphi*cpsi)*_x
    +(-spsi*sphi+ctheta*cphi*cpsi)*_y
    +cpsi*stheta*_z;
  double znew=stheta*sphi*_x-stheta*cphi*_y+ctheta*_z;
  return Vector3d(xnew,ynew,znew);
}
//++
Vector3d Vector3d::InvEuler(double phi, double theta, double psi) const 
//
//    inverse rotation
//-- 
{
  double cpsi=cos(psi);
  double ctheta=cos(theta);
  double cphi=cos(phi);
  double spsi=sin(psi);
  double stheta=sin(theta);
  double sphi=sin(phi);
  double xnew=(cpsi*cphi-ctheta*sphi*spsi)*_x
    -(spsi*cphi+ctheta*sphi*cpsi)*_y
    +sphi*stheta*_z;
  double ynew=(cpsi*sphi+ctheta*cphi*spsi)*_x
    +(-spsi*sphi+ctheta*cphi*cpsi)*_y
    -cphi*stheta*_z;
  double znew=stheta*spsi*_x+stheta*cpsi*_y+ctheta*_z;
  return Vector3d(xnew,ynew,znew);
}
//++
Vector3d Vector3d::Rotate(const Vector3d& omega, double phi)
//
//    rotation of angle phi around an axis omega (Maxwell's rule)
//-- 
{
  Vector3d rotationaxis=omega;
  rotationaxis.Normalize();
  double n=this->Psc(rotationaxis);
  Vector3d myself=*this;
  Vector3d rotate=n*rotationaxis+(myself-n*rotationaxis)*cos(phi)-(myself^rotationaxis)*sin(phi);
  return rotate;
} 
//++  
void Vector3d::Print(ostream& os) const 
//
//--
{
  os << "Vector3: (X,Y,Z)= (" << _x << ", " << _y << ", " << _z 
     << ") --- Theta,Phi= " << _theta << ", " << _phi << "\n"
     << "Norme = " << this->Norm() << endl;
}
//++
// Titre        Operators
//--
//++
Vector3d& Vector3d::operator += (const Vector3d& v) 
//
//-- 
{
  *this=*this+v;
  return *this;
}
//++
Vector3d& Vector3d::operator -= (const Vector3d& v) 
//
//-- 
{
  *this=*this-v;
  return *this;
}
//++
Vector3d& Vector3d::operator += (double d) 
//
//-- 
{
  Setxyz(_x+d,_y+d,_z+d);
  return *this;
}
//++
Vector3d& Vector3d::operator /= (double d) 
//
//-- 
{
  if( d != 0. ) Setxyz(_x/d,_y/d,_z/d);
  else cerr << "Division par zero." << endl;
  return *this;
}
//++
Vector3d& Vector3d::operator *= (double d) 
//
//-- 
{
  Setxyz(_x*d,_y*d,_z*d);
  return *this;
}
//++
Vector3d Vector3d::operator ^ (const Vector3d& v) const 
//
//    vector product
//-- 
{
  return this->Vect(v);
}
//++
Vector3d Vector3d::operator + (double d) const 
//
//-- 
{
  return Vector3d(_x+d,_y+d,_z+d);
}
//++
Vector3d Vector3d::operator + (const Vector3d& v) const 
//
//-- 
{
  return Vector3d(_x+v._x,_y+v._y,_z+v._z);
}
//++
Vector3d Vector3d::operator - (double d) const 
//
//-- 
{
  return *this+(-d);
}
//++
Vector3d Vector3d::operator - (const Vector3d& v) const 
//
//-- 
{
  return *this+(v*(-1.));
}
//++
Vector3d Vector3d::operator * (double d) const 
//
//-- 
{
  return Vector3d(d*_x,d*_y,d*_z);
}
//++
double Vector3d::operator * (const Vector3d& v) const 
//
//    dot product
//-- 
{
  return this->Psc(v);
}
//++
Vector3d Vector3d::operator / (double d) const 
//
//-- 
{
  Vector3d ret=*this;
  if( d != 0. ) ret/=d;
  else  cerr << "Division par zero." << endl;
  return ret;
}
//++
Vector3d& Vector3d::operator = (const Vector3d& v) 
//
//-- 
{
  if( this != &v ) 
    {
      _x=v._x; 
      _y=v._y; 
      _z=v._z; 
      _theta=v._theta; 
      _phi=v._phi;
    }
  return *this;
}
//++
bool Vector3d::operator == (const Vector3d& v) 
//
//-- 
{
  return (this==&v);
}