/*
** (c) 1996-2000 The Regents of the University of California (through
** E.O. Lawrence Berkeley National Laboratory), subject to approval by
** the U.S. Department of Energy.  Your use of this software is under
** license -- the license agreement is attached and included in the
** directory as license.txt or you may contact Berkeley Lab's Technology
** Transfer Department at TTD@lbl.gov.  NOTICE OF U.S. GOVERNMENT RIGHTS.
** The Software was developed under funding from the U.S. Government
** which consequently retains certain rights as follows: the
** U.S. Government has been granted for itself and others acting on its
** behalf a paid-up, nonexclusive, irrevocable, worldwide license in the
** Software to reproduce, prepare derivative works, and perform publicly
** and display publicly.  Beginning five (5) years after the date
** permission to assert copyright is obtained from the U.S. Department of
** Energy, and subject to any subsequent five (5) year renewals, the
** U.S. Government is granted for itself and others acting on its behalf
** a paid-up, nonexclusive, irrevocable, worldwide license in the
** Software to reproduce, prepare derivative works, distribute copies to
** the public, perform publicly and display publicly, and to permit
** others to do so.
*/


//
// $Id: Quaternion.cpp,v 1.12 2002/02/19 20:39:41 vince Exp $
//

// ---------------------------------------------------------------
// Quaternion.cpp
// ---------------------------------------------------------------
#include <cmath>

#include "Quaternion.H"

// ---------------------------------------------------------------
//  These constructors reduce smoothly to the identity when the input
//  points are the same, but will generate Realing-point errors if
//  they are diametrically opposite and the rotation axis is ill-defined.
// ---------------------------------------------------------------


// ---------------------------------------------------------------
AmrQuaternion::AmrQuaternion(const AmrSpherePoint &s1, 
                             const AmrSpherePoint &s2)
{
  Real c(s1 * s2);			//  cosine of rotation angle
  Real xn(Y(s1) * Z(s2) - Z(s1) * Y(s2));   //  unnormalized axis of rotation
  Real yn(Z(s1) * X(s2) - X(s1) * Z(s2));
  Real zn(X(s1) * Y(s2) - Y(s1) * X(s2));
  Real ch(sqrt(0.5 * (1.0 + c)));	//  cosine of half rotation angle
  Real chTemp(0.5 / ch);
  
  w = ch;
  x = xn * chTemp;
  y = yn * chTemp;
  z = zn * chTemp;
}


// ---------------------------------------------------------------
// defines the quaternion that rotates (x1,y1,z1) to (x2,y2,z2)
// ---------------------------------------------------------------
AmrQuaternion::AmrQuaternion(Real x1,Real y1,Real z1,Real x2,Real y2,Real z2) {
  Real c(x1 * x2 + y1 * y2 + z1 * z2);			//  cosine of rotation angle
  Real xn(y1 * z2 - z1 * y2);   //  unnormalized axis of rotation
  Real yn(z1 * x2 - x1 * z2);
  Real zn(x1 * y2 - y1 * x2);
  Real ch(sqrt(0.5 * (1.0 + c)));	//  cosine of half rotation angle
  Real chTemp(0.5 / ch);
  
  w = ch;
  x = xn * chTemp;
  y = yn * chTemp;
  z = zn * chTemp;
}


// ---------------------------------------------------------------
void AmrQuaternion::tomatrix( Real m[4][4] ) const {
  Real wx(w * x);
  Real wy(w * y);
  Real wz(w * z);
  Real xz(x * z);
  Real yz(y * z);
  Real xy(x * y);
  Real xx(x * x);
  Real yy(y * y);
  Real zz(z * z);
  
  m[0][0] = 1.-2.*(yy+zz);  m[0][1] = 2.*(xy-wz);    m[0][2] = 2.*(xz+wy);
  m[1][0] = 2.*(xy+wz);     m[1][1]= 1.-2.*(zz+xx);  m[1][2] = 2.*(yz-wx);
  m[2][0] = 2.*(xz-wy);     m[2][1] = 2.*(yz+wx);    m[2][2] = 1.-2.*(yy+xx);
  
  m[0][3] = m[1][3] = m[2][3] = 0.0;
  m[3][0] = m[3][1] = m[3][2] = 0.0;
  m[3][3] = 1.0;
}


// ---------------------------------------------------------------
Real AmrQuaternion::InfNorm() const {
  Real mat[4][4];
  this->tomatrix(mat);
  Real max(0.0);
  Real sum(0.0);
  for(int i = 0; i < 3; ++i) {
    sum = mat[i][0] + mat[i][1] + mat[i][2];
    max = (sum > max ? sum : max );
  }
  return max;
}
// ---------------------------------------------------------------
// ---------------------------------------------------------------
