/*
** (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.H,v 1.7 2000/11/22 00:23:45 vince Exp $
//

// ---------------------------------------------------------------
// Quaternion.H
// ---------------------------------------------------------------
#ifndef _AMR_QUATERNION_H_
#define _AMR_QUATERNION_H_

#include "REAL.H"
#include "Point.H"

/***************************************************************************

A quaternion represents a rotation of a rigid body in Euclidean
three-dimensional space.  (These quaternions all are normalized
so that the sum of the squares of the components is one.)

If the rotation is counterclockwise by an angle alpha about axis n,
then the corresponding quaternion is

    [  cos(alpha/2),   sin(alpha/2) n  ]

Multiplication of normalized quaternions corresponds to composition of
these rotations:  q * r  means do rotation r first, then q.

The operator  *=  does right multiplication by the argument, so  q *= r
means  q = q*r .  This seems to be more natural for the usual applications
where r is an increment which is successively applied to a position q.

***************************************************************************/

class AmrQuaternion
{
  Real w, x, y, z;
public:
  AmrQuaternion()  { w=1.;  x=y=z=0.; }		//  The identity rotation
  AmrQuaternion(Real W, Real X, Real Y, Real Z)  { w=W; x=X; y=Y; z=Z; }
  AmrQuaternion(Real, Real, Real, Real, Real, Real);
  //returns a quaternion that rotates p1 to p2
  AmrQuaternion(const AmrSpherePoint &p1, const AmrSpherePoint &p2);
  AmrQuaternion operator *(const AmrQuaternion &q) const;//right-mult by q
  AmrQuaternion operator /(const AmrQuaternion &q) const;//right-mult by q^-1
  AmrQuaternion operator *=(const AmrQuaternion &q);	//right-mult by q
  AmrQuaternion operator /=(const AmrQuaternion &q);	//right-mult by q^-1
  friend AmrQuaternion inverse(const AmrQuaternion &q);//  inverse
  
  Real InfNorm() const;
  void tomatrix( Real m[4][4] ) const;

  Real QW() const { return w; }
  Real QX() const { return x; }
  Real QY() const { return y; }
  Real QZ() const { return z; }
  
  friend ostream& operator<<( ostream &s, const AmrQuaternion &q);
};

inline AmrQuaternion AmrQuaternion::operator *(const AmrQuaternion &q) const
{
  return AmrQuaternion(	w*q.w - x*q.x - y*q.y - z*q.z,
			x*q.w + w*q.x + y*q.z - z*q.y,
			y*q.w + w*q.y + z*q.x - x*q.z,
			z*q.w + w*q.z + x*q.y - y*q.x );
}

inline AmrQuaternion AmrQuaternion::operator /(const AmrQuaternion &q) const
{
  return AmrQuaternion(	w*q.w + x*q.x + y*q.y + z*q.z,
			x*q.w - w*q.x - y*q.z + z*q.y,
			y*q.w - w*q.y - z*q.x + x*q.z,
			z*q.w - w*q.z - x*q.y + y*q.x );
}

inline AmrQuaternion AmrQuaternion::operator *=(const AmrQuaternion &q)
{
  Real ww = w*q.w - x*q.x - y*q.y - z*q.z;
  Real xx = x*q.w + w*q.x + y*q.z - z*q.y;
  Real yy = y*q.w + w*q.y + z*q.x - x*q.z;
  Real zz = z*q.w + w*q.z + x*q.y - y*q.x;
  w=ww;  x=xx;  y=yy;  z=zz;
  return *this;
}

inline AmrQuaternion AmrQuaternion::operator /=(const AmrQuaternion &q)
{
  Real ww = w*q.w + x*q.x + y*q.y + z*q.z;
  Real xx = x*q.w - w*q.x - y*q.z + z*q.y;
  Real yy = y*q.w - w*q.y - z*q.x + x*q.z;
  Real zz = z*q.w - w*q.z - x*q.y + y*q.x;
  w=ww;  x=xx;  y=yy;  z=zz;
  return *this;
}

inline AmrQuaternion inverse(const AmrQuaternion &q)
{
  return AmrQuaternion( q.w, -q.x, -q.y, -q.z );
}

inline ostream& operator << ( ostream &s, const AmrQuaternion &q)
{
  return s << "AmrQuaternion( " << q.w << " , " << q.x << " , "
           << q.y << " , " << q.z << " )";
}

#endif  //  ndef _AMR_QUATERNION_H_

