// transf.h
// by Glenn G. Chappell <chappellg@member.ams.org>
// and Chris Hartman    <hartman@cs.uaf.edu>
// VERSION 0.6
// 23 Jan 2004
//
// Part of the TRANSF package
// Copyright (c) 2003 Glenn G. Chappell and Chris Hartman.
// This file, and the TRANSF package, are provided with no warranties
// whatsoever. The TRANSF package may be freely copied, modified, and used
// for any purpose, as long as the following conditions are met.
// (1) This notice must be retained.
// (2) If the TRANSF files are modified, or if portions of the files are
//     reused in other files, then the modified/reused code must be marked
//     as such.
//
// This file defines classes
//    tf::rot
//    tf::orient
//    tf::transf
//    tf::frameref
// and the class
//    tf::tfquat
// along with associated global functions in namespace tf.

#ifndef TF_FILE_TRANSF_H_INCLUDED
#define TF_FILE_TRANSF_H_INCLUDED

// ************************************************************************
// Includes & macro definitions
// ************************************************************************

#include "vecpos.h"  // For tf::vec, tf::pos, associated global func's

// All macros ending in "_INTERNAL" are #undef'd at end of file.

#ifdef __sgi
# include <math.h>  // For cos, sin, acos, sqrt
# define TF_COS_INTERNAL (cos)
# define TF_SIN_INTERNAL (sin)
# define TF_ACOS_INTERNAL (acos)
# define TF_SQRT_INTERNAL (sqrt)
#else
# include <cmath>    // For std::cos, std::sin, std::acos, std::sqrt
# if defined(_MSC_VER) && (_MSC_VER <= 1200)
   // MS-Vis C++ < 7.0 <cmath> does not use namespace std
#  define TF_COS_INTERNAL (cos)
#  define TF_SIN_INTERNAL (sin)
#  define TF_ACOS_INTERNAL (acos)
#  define TF_SQRT_INTERNAL (sqrt)
# else
#  define TF_COS_INTERNAL (std::cos)
#  define TF_SIN_INTERNAL (std::sin)
#  define TF_ACOS_INTERNAL (std::acos)
#  define TF_SQRT_INTERNAL (std::sqrt)
# endif
#endif

#define TF_PI_INTERNAL (3.141592653589793238462643383279502884197169)

#define TF_INLINE_INTERNAL inline
   // Dirty trick to avoid redefined-function compiler complaints

// ************************************************************************
// Begin namespace tf
// ************************************************************************

namespace tf {

// ************************************************************************
// class tfquat - Declaration
// ************************************************************************

// class tfquat
// Low-feature quaternion class, for modeling rotations.
class tfquat {

// ***** tfquat: types *****
public:
   typedef vec::value_type value_type;

// ***** tfquat: friend declarations *****
   friend bool operator==(const tfquat & lhs,        // Uses re_, pu_
                          const tfquat & rhs);
   friend bool operator!=(const tfquat & lhs,        // Uses re_, pu_
                          const tfquat & rhs);
   friend tfquat operator-(const tfquat & thequat);  // Uses re_, pu_

// ***** tfquat: internal-use utility functions *****
private:
   static value_type valuesqrt(value_type s)
   { return TF_SQRT_INTERNAL(s); }

// ***** tfquat: ctors, dctor, op=, swap *****
public:
   // It might seem reasonable for the constructor below to be used for
   // implicit conversions. However, consider things like constructing a
   // rot given a double. The double should be considered as the measure
   // of an angle in degrees, *not* as a tfquat with the same value.
   explicit tfquat(value_type thereal = 0.,
                   const vec & thepure = vec(0.,0.,0.))
      :re_(thereal),pu_(thepure)
   {}
   // Copy ctor, copy assn, dctor: use compiler-generated
   void swap(tfquat & other)
   {
      value_type tmpr = re_;
      re_ = other.re_;
      other.re_ = tmpr;

      pu_.swap(other.pu_);
   }

// ***** tfquat: data access ****
public:
   const value_type & real() const
   { return re_; }
   const vec & pure() const
   { return pu_; }
   const value_type & i() const
   { return pu_.x(); }
   const value_type & j() const
   { return pu_.y(); }
   const value_type & k() const
   { return pu_.z(); }

// ***** tfquat: arithmetic/manipulation *****
public:
   tfquat conj() const
   { return tfquat(re_, -pu_); }
   tfquat & operator*=(const tfquat & rhs)
   {
      const value_type newr = re_*rhs.re_ - dot(pu_,rhs.pu_);
      pu_ = re_*rhs.pu_ + rhs.re_*pu_ + cross(pu_, rhs.pu_);
      re_ = newr;
      return *this;
   }
   tfquat & operator/=(const tfquat & rhs)  // Unsafe; may divide by zero
   {
      const value_type magsqr = rhs.re_*rhs.re_ + rhs.pu_.lengthsqr();
      const value_type newr = (re_*rhs.re_ + dot(pu_, rhs.pu_)) / magsqr;
      pu_ = (-re_*rhs.pu_ + rhs.re_*pu_ - cross(pu_, rhs.pu_)) / magsqr;
      re_ = newr;
      return *this;
   }
   tfquat & operator/=(double rhs)          // Unsafe; may divide by zero
   {
      pu_ /= rhs;
      re_ /= rhs;
      return *this;
   }
   tfquat normalized() const
   {
      const value_type magsqr = re_*re_ + pu_.lengthsqr();
      return (magsqr != 0.) ? tfquat(*this) /= valuesqrt(magsqr)
                            : tfquat(1.);
   }

// **** tfquat: data members ****
private:
   value_type re_;  // Real part
   vec pu_;         // Pure (vector) part

};  // End class tfquat

// ************************************************************************
// class tfquat - Related global operators
// ************************************************************************

TF_INLINE_INTERNAL
bool operator==(const tfquat & lhs,       // friend of class tfquat
                const tfquat & rhs)
{ return lhs.re_ == rhs.re_ && lhs.pu_ == rhs.pu_; }

TF_INLINE_INTERNAL
bool operator!=(const tfquat & lhs,       // friend of class tfquat
                const tfquat & rhs)
{ return lhs.re_ != rhs.re_ || lhs.pu_ != rhs.pu_; }

TF_INLINE_INTERNAL
tfquat operator*(const tfquat & lhs,
                 const tfquat & rhs)
{ return tfquat(lhs) *= rhs; }

TF_INLINE_INTERNAL
tfquat operator/(const tfquat & lhs,      // Unsafe
                 const tfquat & rhs)      // May divide by zero
{ return tfquat(lhs) /= rhs; }

TF_INLINE_INTERNAL
tfquat operator-(const tfquat & thequat)  // friend of class tfquat
{ return tfquat(-thequat.re_, -thequat.pu_); }

// ************************************************************************
// class tfquat - Related global functions
// ************************************************************************

TF_INLINE_INTERNAL
void swap(tfquat & q1, tfquat & q2)
{ q1.swap(q2); }

// ************************************************************************
// class rot - Declaration
// ************************************************************************

// class rot
// 3-D rotation.
class rot {

// ***** rot: types *****
public:
   typedef vec::value_type value_type;

// ***** rot: friend declarations *****
   friend class orient;
   friend bool operator==(const rot & lhs,  // Uses q_
                          const rot & rhs);
   friend bool operator!=(const rot & lhs,  // Uses q_
                          const rot & rhs);
   friend rot rotfromto(const vec & start,  // Uses rotfromto (member)
                        const vec & end);

// ***** rot: internal-use utility functions *****
private:
   // These are also used by class orient.
   static value_type valuecos_deg(value_type s)
   { return TF_COS_INTERNAL(TF_PI_INTERNAL/180.*s); }
   static value_type valuesin_deg(value_type s)
   { return TF_SIN_INTERNAL(TF_PI_INTERNAL/180.*s); }
   static value_type valueacos_deg(value_type s)
   { return 180./TF_PI_INTERNAL*TF_ACOS_INTERNAL(s); }
   static value_type valuesqrt(value_type s)
   { return TF_SQRT_INTERNAL(s); }
   // Below is like vec::normalized, except that (0,0,0) normalized
   // is (0,0,1). We use this to normalize axes of rotation, in
   // order to make rotation handling in 2-D work a bit better.
   static vec axis_normalized(const vec & theaxis)
   {
      const value_type lensqr = theaxis.lengthsqr();
      return (lensqr != 0.) ? theaxis/valuesqrt(lensqr)
                            : vec(0.,0.,1.);
   }
   // Each rotation corresponds to two different quaternions. Given a
   // quaternion representing a rotation, we return the "canonical" one.
   // Specifically, the real part of the returned quaternion is
   // nonnegative. If the real part is zero, then we have a 180-degree
   // rotation, and one of the two possible axes must be chosen. It really
   // does not matter how we do this, as long as we are consistent.
   static tfquat canonicalq(const tfquat & theq)
   {
      if (theq.real() < 0.)
         return -theq;
      else if (theq.real() > 0.)
         return theq;
      if (theq.i() < 0.)
         return -theq;
      else if (theq.i() > 0.)
         return theq;
      if (theq.j() < 0.)
         return -theq;
      else if (theq.j() > 0.)
         return theq;
      if (theq.k() < 0.)
         return -theq;
      else
         return theq;
   }

// ***** rot: ctors, dctor, op=, swap *****
public:
   explicit rot(value_type angle_deg = 0.,
                const vec & axis = vec(0.,0.,1.))
      :q_(tfquat(valuecos_deg(angle_deg/2.),
                 valuesin_deg(angle_deg/2.)*axis_normalized(axis)))
   {}
   inline explicit rot(const orient & o);
      // Implemented outside of class declaration
   explicit rot(const tfquat & theq)
      :q_(theq.normalized())
   {}
   // Copy ctor, copy assn, dctor: use compiler-generated
   void swap(rot & other)
   { q_.swap(other.q_); }

// ***** rot: functions dealing with zero *****
public:
   static rot zero()
   { return rot(tfquat(1., vec::zero())); }

// ***** rot: arithmetic/manipulation *****
public:
   rot & operator*=(const rot & rhs)
   { q_ *= rhs.q_; return *this; }
   rot & operator/=(const rot & rhs)  // Safe; will not divide by zero
   { q_ /= rhs.q_; return *this; }

// ***** rot: conversions *****
public:
   tfquat quat() const  // returns CANONICAL quaternion
   { return canonicalq(q_); }
   value_type cosangle() const
   {
      const value_type cosang = 2. * q_.real() * q_.real() - 1.;
      // Due to minor numerical errors, cosang may actually be slightly
      // greater than 1 (it should not be less than -1). We fix this, to
      // obtain a value that we can take the arccosine of.
      return (cosang > 1.) ? 1. : cosang;
   }
   value_type angle() const
   { return valueacos_deg(cosangle()); }
   vec axis() const
   { return axis_normalized(quat().pure()); }  // quat(): needs canonical

// ***** rot: functions for rot's as transformations *****
public:
   rot & compose_assign(const rot & rhs)
   { return *this *= rhs; }
   rot & lcompose_assign(const rot & rhs)
   { return *this = (rot(rhs) *= *this); }
   rot compose(const rot & other) const
   { return rot(*this) *= other; }
   vec applyto(const vec & operand) const
   { return (q_ * tfquat(0., operand) / q_).pure(); }
   pos applyto(const pos & operand) const
   { return pos(applyto(vec(operand))); }
   rot applyto(const rot & operand) const
   { return rot(*this) *= operand; }
   inline orient applyto(const orient & operand) const;
      // Implemented outside of class declaration
   rot inverse() const
   { return rot(q_.conj()); }
   rot power(value_type s) const
   { return rot(s*angle(), axis()); }

   // Column-major ordering matrix creation
#define TF_MM16C_ROT_BODY_INTERNAL(T)               \
   const value_type w = q_.real();                  \
   const value_type x = q_.i();                     \
   const value_type y = q_.j();                     \
   const value_type z = q_.k();                     \
   const value_type w2=w*w, x2=x*x, y2=y*y, z2=z*z; \
   m[ 0]=T(w2+x2-y2-z2);   m[ 4]=T(2.*x*y-2.*w*z);  \
         m[ 8]=T(2.*x*z+2.*w*y); m[12]=T(0.);       \
   m[ 1]=T(2.*x*y+2.*w*z); m[ 5]=T(w2-x2+y2-z2);    \
         m[ 9]=T(2.*y*z-2.*w*x); m[13]=T(0.);       \
   m[ 2]=T(2.*x*z-2.*w*y); m[ 6]=T(2.*y*z+2.*w*x);  \
         m[10]=T(w2-x2-y2+z2);   m[14]=T(0.);       \
   m[ 3]=T(0.);            m[ 7]=T(0.);             \
         m[11]=T(0.);            m[15]=T(1.);

   void makematrix16c(double * m) const
   { TF_MM16C_ROT_BODY_INTERNAL(double) }
   void makematrix16c(float * m) const
   { TF_MM16C_ROT_BODY_INTERNAL(float) }
      // "float" here eliminates double-to-float conversion warnings
   template<typename M>
   void makematrix16c(M & m) const
   { TF_MM16C_ROT_BODY_INTERNAL(double) }
   template<typename M>
   void makematrix16c(const M & m) const
   { TF_MM16C_ROT_BODY_INTERNAL(double) }
#undef TF_MM16C_ROT_BODY_INTERNAL

   // Row-major ordering matrix creation
#define TF_MM16R_ROT_BODY_INTERNAL(T)               \
   const value_type w = q_.real();                  \
   const value_type x = q_.i();                     \
   const value_type y = q_.j();                     \
   const value_type z = q_.k();                     \
   const value_type w2=w*w, x2=x*x, y2=y*y, z2=z*z; \
   m[ 0]=T(w2+x2-y2-z2);   m[ 1]=T(2.*x*y-2.*w*z);  \
         m[ 2]=T(2.*x*z+2.*w*y); m[ 3]=T(0.);       \
   m[ 4]=T(2.*x*y+2.*w*z); m[ 5]=T(w2-x2+y2-z2);    \
         m[ 6]=T(2.*y*z-2.*w*x); m[ 7]=T(0.);       \
   m[ 8]=T(2.*x*z-2.*w*y); m[ 9]=T(2.*y*z+2.*w*x);  \
         m[10]=T(w2-x2-y2+z2);   m[11]=T(0.);       \
   m[12]=T(0.);            m[13]=T(0.);             \
         m[14]=T(0.);            m[15]=T(1.);

   void makematrix16r(double * m) const
   { TF_MM16R_ROT_BODY_INTERNAL(double) }
   void makematrix16r(float * m) const
   { TF_MM16R_ROT_BODY_INTERNAL(float) }
      // "float" here eliminates double-to-float conversion warnings
   template<typename M>
   void makematrix16r(M & m) const
   { TF_MM16R_ROT_BODY_INTERNAL(double) }
   template<typename M>
   void makematrix16r(const M & m) const
   { TF_MM16R_ROT_BODY_INTERNAL(double) }
#undef TF_MM16R_ROT_BODY_INTERNAL

// ***** rot: implementation for long-ish global functions
private:
   static rot rotfromto(const vec & start,
                        const vec & end)
   {
      // This is an easy computation, but it has lots of special cases.
      const value_type startlensqr = start.lengthsqr();
      const value_type endlensqr = end.lengthsqr();

      // Special case 1: At least one of the vectors has zero length.
      //    Result: Identity rotation.
      if (startlensqr == 0. || endlensqr == 0.)
         return zero();

      const value_type ndotp = dot(start/valuesqrt(startlensqr),
                                   end/valuesqrt(endlensqr));
      const vec crossp = cross(start, end);
      const value_type crossplensqr = crossp.lengthsqr();

      if (crossplensqr == 0.)
      {
         if (ndotp >= 0.)
            // Special case 2: Non-zero vectors with the same direction.
            //    Result: Identity rotation.
            return zero();

         // Special case 3: Non-zero vectors with opposite directions.
         //    Result: 180-degree rotation about some axis perpendicular to
         //    the vectors. Prefer an axis pointing in the +z direction, in
         //    order to make rotations work nicely in 2-D.
         vec axis = vec(0., 0., 1.) - vec(0., 0., 1.).component(start);
         if (axis.lengthsqr() == 0.)
            axis = vec(1., 0., 0.);
         return rot(tfquat(0., axis));
            // 180-degree rot about axis (quaternion will be normalized)
      }

      // Lastly, the non-special case.
      const value_type coshalfangsqr = (1. + ndotp)/2.;
      const value_type cpcoefsqr = (1. - coshalfangsqr)/crossplensqr;
      return rot(tfquat(
         (coshalfangsqr < 0.) ? 0. : valuesqrt(coshalfangsqr),
         (cpcoefsqr < 0.) ? vec::zero() : valuesqrt(cpcoefsqr) * crossp));
         // Deal with possible minor numerical errors
   }

// ***** rot: data members *****
private:
   tfquat q_;  // A quaternion representing this rotation

};  // End class rot

// ************************************************************************
// class rot - Related global operators
// ************************************************************************

// Note: operator== and operator!= are included here for completeness, but
// there is really no good reason to use them. To check whether two
// rot's r1, r2 are equal or nearly equal, use something like
//    (r1/r2).angle() < epsilon
// where epsilon is a small, positive floating-point number (in degrees).

TF_INLINE_INTERNAL
bool operator==(const rot & lhs,  // friend of class rot
                const rot & rhs)
{ return lhs.q_ == rhs.q_; }  // Ick! There's no good way to do this!

TF_INLINE_INTERNAL
bool operator!=(const rot & lhs,  // friend of class rot
                const rot & rhs)
{ return lhs.q_ != rhs.q_; }  // Ick! There's no good way to do this!

TF_INLINE_INTERNAL
rot operator*(const rot & lhs,
              const rot & rhs)
{ return rot(lhs) *= rhs; }

TF_INLINE_INTERNAL
rot operator/(const rot & lhs,    // Safe; will not divide by zero
              const rot & rhs)
{ return rot(lhs) /= rhs; }

// ************************************************************************
// class rot - Related global functions
// ************************************************************************

TF_INLINE_INTERNAL
void swap(rot & r1, rot & r2)
{ r1.swap(r2); }

TF_INLINE_INTERNAL
rot compose(const rot & lhs,
            const rot & rhs)
{ return lhs.compose(rhs); }

TF_INLINE_INTERNAL
rot power(const rot & therot,
          rot::value_type s)
{ return therot.power(s); }

TF_INLINE_INTERNAL
vec applyto(const rot & therot,
            const vec & operand)
{ return therot.applyto(operand); }

TF_INLINE_INTERNAL
pos applyto(const rot & therot,
            const pos & operand)
{ return therot.applyto(operand); }

TF_INLINE_INTERNAL
rot applyto(const rot & therot,
            const rot & operand)
{ return therot.applyto(operand); }

TF_INLINE_INTERNAL
rot rotfromto(const vec & start,
              const vec & end)
{ return rot::rotfromto(start, end); }

TF_INLINE_INTERNAL
rot interpolate(const rot & a,
                const rot & b,
                rot::value_type t)
{ return (b/a).power(t) * a; }

// ************************************************************************
// class orient - Declaration
// ************************************************************************

// class orient
// 3-D orientation.
// Absolute version of class rot.
class orient {

// ***** orient: types *****
public:
   typedef rot::value_type value_type;

// ***** orient: friend declarations *****
   friend class rot;
   friend bool operator==(const orient & lhs,  // Uses q_
                          const orient & rhs);
   friend bool operator!=(const orient & lhs,  // Uses q_
                          const orient & rhs);

// ***** orient: ctors, dctor, op=, swap *****
public:
   explicit orient(value_type angle_deg = 0.,
                   const vec & axis = vec(0.,0.,1.))
      :q_(tfquat(
         rot::valuecos_deg(angle_deg/2.),
         rot::valuesin_deg(angle_deg/2.)*rot::axis_normalized(axis)))
   {}
   explicit orient(const rot & r)
      :q_(r.q_)
   {}
   explicit orient(const tfquat & theq)
      :q_(theq.normalized())
   {}
   // Copy ctor, copy assn, dctor: use compiler-generated
   void swap(orient & other)
   { q_.swap(other.q_); }

// ***** orient: functions dealing with zero *****
public:
   static orient zero()
   { return orient(tfquat(1., vec::zero())); }

// ***** orient: conversions *****
public:
   tfquat quat() const  // returns CANONICAL quaternion
   { return rot::canonicalq(q_); }
   value_type cosangle() const
   {
      const value_type cosang = 2. * q_.real() * q_.real() - 1.;
      // Due to minor numerical errors, cosang may actually be slightly
      // greater than 1 (it should not be less than -1). We fix this, to
      // obtain a value that we can take the arccosine of.
      return (cosang > 1.) ? 1. : cosang;
   }
   value_type angle() const
   { return rot::valueacos_deg(cosangle()); }
   vec axis() const
   { return rot::axis_normalized(quat().pure()); }
      // quat(): needs canonical

// ***** orient: data members *****
private:
   tfquat q_;  // A quaternion representing this orientation

};  // End class orient

// ************************************************************************
// class orient - Related global operators
// ************************************************************************

// Note: operator== and operator!= are included here for completeness, but
// there is really no good reason to use them. To check whether two
// orient's o1, o2 are equal or nearly equal, use something like
//    (o1/o2).angle() < epsilon
// where epsilon is a small, positive floating-point number (in degrees).

TF_INLINE_INTERNAL
bool operator==(const orient & lhs,  // friend of class orient
                const orient & rhs)
{ return lhs.q_ == rhs.q_; }  // Ick! There's no good way to do this!

TF_INLINE_INTERNAL
bool operator!=(const orient & lhs,  // friend of class orient
                const orient & rhs)
{ return lhs.q_ != rhs.q_; }  // Ick! There's no good way to do this!

TF_INLINE_INTERNAL
orient operator*(const rot & lhs,
                 const orient & rhs)
{ return orient(lhs * rot(rhs)); }

TF_INLINE_INTERNAL
rot operator/(const orient & lhs,    // Safe; will not divide by zero
              const orient & rhs)
{ return rot(lhs)/rot(rhs); }

// ************************************************************************
// class orient - Related global functions
// ************************************************************************

TF_INLINE_INTERNAL
void swap(orient & o1, orient & o2)
{ o1.swap(o2); }

TF_INLINE_INTERNAL
orient applyto(const rot & therot,
               const orient & operand)
{ return therot.applyto(operand); }

TF_INLINE_INTERNAL
orient interpolate(const orient & a,
                   const orient & b,
                   orient::value_type t)
{ return (b/a).power(t) * a; }

// ************************************************************************
// class rot - Member functions involving orient
// ************************************************************************

inline rot::rot(const orient & o)  // explicit
   :q_(o.q_)
{}

inline orient rot::applyto(const orient & theorient) const
{ return orient(applyto(rot(theorient))); }

// ************************************************************************
// class transf - Declaration
// ************************************************************************

// class transf
// 3-D rigid transformation.
class transf {

// ***** transf: types *****
public:
   typedef vec::value_type value_type;

// ***** transf: friend declarations *****
   friend class frameref;
   friend bool operator==(const transf & lhs,  // Uses v_ & r_
                          const transf & rhs);
   friend bool operator!=(const transf & lhs,  // Uses v_ & r_
                          const transf & rhs);

// ***** transf: internal-use utility functions *****
private:
   // We also put utility functions for class frameref here.
   // (But there are none at present.)
   static value_type valuesin_deg(value_type s)
   { return TF_SIN_INTERNAL(TF_PI_INTERNAL/180.*s); }
   static value_type valuesqrt(value_type s)
   { return TF_SQRT_INTERNAL(s); }

// ***** transf: ctors, dctor, op=, swap *****
public:
   explicit transf(const vec & thev = vec::zero(),
                   const rot & ther = rot::zero())
      :v_(thev),r_(ther)
   {}
   explicit transf(const rot & ther,
                   const vec & thev = vec::zero())
      :v_(thev),r_(ther)
   {}
   inline explicit transf(const frameref & f);
      // Implemented outside of class declaration
   // Copy ctor, copy assn, dctor: use compiler-generated
   void swap(transf & other)
   { v_.swap(other.v_); r_.swap(other.r_); }

// ***** transf: functions dealing with zero *****
public:
   static transf zero()
   { return transf(vec::zero(), rot::zero()); }

// ***** transf: arithmetic/manipulation *****
public:
   transf & operator*=(const transf & rhs)
   {
      v_ += r_.applyto(rhs.v_);
      r_ *= rhs.r_;
      return *this;
   }
   transf & operator/=(const transf & rhs)  // Safe;
                                            // will not divide by zero
   {
      r_ /= rhs.r_;
      v_ -= r_.applyto(rhs.v_);
      return *this;
   }

// ***** transf: conversions *****
public:
   vec vecpart() const
   { return v_; }
   rot rotpart() const
   { return r_; }

   // The following are defined because of the guarantee that code
   // involving absolute types (pos, orient, frameref) will continue to
   // work if ALL absolute types are replaced with the corresponding
   // relative types (vec, rot, transf). Since these member functions are
   // defined in class frameref, they must also be defined here.
   vec pospart() const
   { return v_; }
   rot orientpart() const
   { return r_; }

// ***** transf: functions for transf's as transformations *****
public:
   transf & compose_assign(const transf & rhs)
   { return *this *= rhs; }
   transf & lcompose_assign(const transf & rhs)
   { return *this = (transf(rhs) *= *this); }
   transf compose(const transf & other) const
   { return transf(*this) *= other; }
   vec applyto(const vec & operand) const
   { return v_ + r_.applyto(operand); }
   pos applyto(const pos & operand) const
   { return v_ + r_.applyto(operand); }
   transf applyto(const transf & operand) const
   { return transf(*this) *= operand; }
   inline frameref applyto(const frameref & operand) const;
      // Implemented outside of class declaration
   transf inverse() const            // Safe; will not divide by zero
   {
      rot rinv = r_.inverse();
      return transf(rinv.applyto(-v_), rinv);
   }
   transf power(value_type s) const  // Safe; will not divide by zero
   {
      const vec rpure = r_.quat().pure();
         // quat(): does not need canonical
      const value_type sinhalfangsqr = rpure.lengthsqr();
      if (sinhalfangsqr == 0.)
         return transf(s*v_);
      const vec par = v_.component(rpure);  // rpure is non-zero
      return transf(
         s*par + valuesin_deg(s*r_.angle()/2.)/valuesqrt(sinhalfangsqr)
               * r_.power((s-1.)/2.).applyto(v_-par),
         r_.power(s));
   }

   // Column-major ordering matrix creation
#define TF_MM16C_TRANSF_BODY_INTERNAL(T)            \
   const tfquat qq = r_.quat();                     \
      /* quat(): does not need canonical */         \
   const value_type w = qq.real();                  \
   const value_type x = qq.i();                     \
   const value_type y = qq.j();                     \
   const value_type z = qq.k();                     \
   const value_type w2=w*w, x2=x*x, y2=y*y, z2=z*z; \
   m[ 0]=T(w2+x2-y2-z2);   m[ 4]=T(2*x*y-2*w*z);    \
         m[ 8]=T(2.*x*z+2.*w*y); m[12]=T(v_[0]);    \
   m[ 1]=T(2.*x*y+2.*w*z); m[ 5]=T(w2-x2+y2-z2);    \
         m[ 9]=T(2.*y*z-2.*w*x); m[13]=T(v_[1]);    \
   m[ 2]=T(2.*x*z-2.*w*y); m[ 6]=T(2.*y*z+2.*w*x);  \
         m[10]=T(w2-x2-y2+z2);   m[14]=T(v_[2]);    \
   m[ 3]=T(0.);            m[ 7]=T(0.);             \
         m[11]=T(0.);            m[15]=T(1.);

   void makematrix16c(double * m) const
   { TF_MM16C_TRANSF_BODY_INTERNAL(double) }
   void makematrix16c(float * m) const
   { TF_MM16C_TRANSF_BODY_INTERNAL(float) }
      // "float" here eliminates double-to-float conversion warnings
   template<typename M>
   void makematrix16c(M & m) const
   { TF_MM16C_TRANSF_BODY_INTERNAL(double) }
   template<typename M>
   void makematrix16c(const M & m) const
   { TF_MM16C_TRANSF_BODY_INTERNAL(double) }
#undef TF_MM16C_TRANSF_BODY_INTERNAL

   // Row-major ordering matrix creation
#define TF_MM16R_TRANSF_BODY_INTERNAL(T)            \
   const tfquat qq = r_.quat();                     \
      /* quat(): does not need canonical */         \
   const value_type w = qq.real();                  \
   const value_type x = qq.i();                     \
   const value_type y = qq.j();                     \
   const value_type z = qq.k();                     \
   const value_type w2=w*w, x2=x*x, y2=y*y, z2=z*z; \
   m[ 0]=T(w2+x2-y2-z2);   m[ 1]=T(2.*x*y-2.*w*z);  \
         m[ 2]=T(2.*x*z+2.*w*y); m[ 3]=T(v_[0]);    \
   m[ 4]=T(2.*x*y+2.*w*z); m[ 5]=T(w2-x2+y2-z2);    \
         m[ 6]=T(2.*y*z-2.*w*x); m[ 7]=T(v_[1]);    \
   m[ 8]=T(2.*x*z-2.*w*y); m[ 9]=T(2.*y*z+2.*w*x);  \
         m[10]=T(w2-x2-y2+z2);   m[11]=T(v_[2]);    \
   m[12]=T(0.);            m[13]=T(0.);             \
         m[14]=T(0.);            m[15]=T(1.);

   void makematrix16r(double * m) const
   { TF_MM16R_TRANSF_BODY_INTERNAL(double) }
   void makematrix16r(float * m) const
   { TF_MM16R_TRANSF_BODY_INTERNAL(float) }
      // "float" here eliminates double-to-float conversion warnings
   template<typename M>
   void makematrix16r(M & m) const
   { TF_MM16R_TRANSF_BODY_INTERNAL(double) }
   template<typename M>
   void makematrix16r(const M & m) const
   { TF_MM16R_TRANSF_BODY_INTERNAL(double) }
#undef TF_MM16R_TRANSF_BODY_INTERNAL

// ***** transf: data members *****
private:
   vec v_;  // Translation part
   rot r_;  // Rotation part
   // Transformation is r_ then v_, i.e., transf(v_) * transf(r_).

};  // End class transf

// ************************************************************************
// class transf - Related global operators
// ************************************************************************

// Note: operator== and operator!= are included here for completeness, but
// there is really no good reason to use them. In fact, even checking
// near-equality of transf's is always a little problematic. One
// not-too-bad way to check whether two transf's t1, t2 are equal or
// nearly equal, is to use something like
//    (t1/t2).rotpart().angle() < epsilon1
//       && (t1/t2).vecpart().norminf() < epsilon2
// where epsilon1, epsilon2 are small, positive floating-point numbers
// (epsilon1 is in degrees, and epsilon2 is in spacial units).

TF_INLINE_INTERNAL
bool operator==(const transf & lhs,   // friend of class transf
                const transf & rhs)
{ return lhs.v_ == rhs.v_ && lhs.r_ == rhs.r_; }

TF_INLINE_INTERNAL
bool operator!=(const transf & lhs,   // friend of class transf
                const transf & rhs)
{ return lhs.v_ != rhs.v_ || lhs.r_ != rhs.r_; }

TF_INLINE_INTERNAL
transf operator*(const transf & lhs,
                 const transf & rhs)
{ return transf(lhs) *= rhs; }

TF_INLINE_INTERNAL
transf operator/(const transf & lhs,  // Safe; will not divide by zero
                 const transf & rhs)
{ return transf(lhs) /= rhs; }

// ************************************************************************
// class transf - Related global functions
// ************************************************************************

TF_INLINE_INTERNAL
void swap(transf & t1, transf & t2)
{ t1.swap(t2); }

TF_INLINE_INTERNAL
transf compose(const transf & lhs,
               const transf & rhs)
{ return lhs.compose(rhs); }

TF_INLINE_INTERNAL
transf power(const transf & thetransf,  // Safe; will not divide by zero
             transf::value_type s)
{ return thetransf.power(s); }

TF_INLINE_INTERNAL
vec applyto(const transf & thetransf,
            const vec & operand)
{ return thetransf.applyto(operand); }

TF_INLINE_INTERNAL
pos applyto(const transf & thetransf,
            const pos & operand)
{ return thetransf.applyto(operand); }

TF_INLINE_INTERNAL
transf applyto(const transf & thetransf,
               const transf & operand)
{ return thetransf.applyto(operand); }

TF_INLINE_INTERNAL
transf interpolate(const transf & a,    // Safe; will not divide by zero
                   const transf & b,
                   transf::value_type t)
{ return (b/a).power(t) * a; }

// ************************************************************************
// class frameref - Declaration
// ************************************************************************

// class frameref
// 3-D frame of reference.
// Absolute version of class transf.
class frameref {

// ***** frameref: types *****
public:
   typedef vec::value_type value_type;

// ***** frameref: friend declarations *****
   friend class transf;
   friend bool operator==(const frameref & lhs,  // Uses p_ & o_
                          const frameref & rhs);
   friend bool operator!=(const frameref & lhs,  // Uses p_ & o_
                          const frameref & rhs);

// ***** frameref: ctors, dctor, op=, swap *****
public:
   explicit frameref(const pos & thep = pos::zero(),
                     const orient & theo = orient::zero())
      :p_(thep),o_(theo)
   {}
   explicit frameref(const orient & theo,
                     const pos & thep = pos::zero())
      :p_(thep),o_(theo)
   {}
   explicit frameref(const transf & t)
      :p_(t.v_),o_(t.r_)
   {}
   // Copy ctor, copy assn, dctor: use compiler-generated
   void swap(frameref & other)
   { p_.swap(other.p_); o_.swap(other.o_); }

// ***** frameref: functions dealing with zero *****
public:
   static frameref zero()
   { return frameref(pos::zero(), orient::zero()); }

// ***** frameref: conversions *****
public:
   pos pospart() const
   { return p_; }
   orient orientpart() const
   { return o_; }

// ***** frameref: data members *****
private:
   // Below, we represent a frame of reference as position + orientation in
   // the same way that we represent a transformation as translation +
   // rotation. See class transf.
   pos p_;     // Position part
   orient o_;  // Orientation part

};  // End class frameref

// ************************************************************************
// class frameref - Related global operators
// ************************************************************************

// Note: operator== and operator!= are included here for completeness, but
// there is really no good reason to use them. In fact, even checking
// near-equality of frameref's is always a little problematic. One
// not-too-bad way to check whether two fraemref's f1, f2 are equal or
// nearly equal, is to use something like
//    (f1/f2).rotpart().angle() < epsilon1
//       && (f1/f2).vecpart().norminf() < epsilon2
// where epsilon1, epsilon2 are small, positive floating-point numbers
// (epsilon1 is in degrees, and epsilon2 is in spacial units).

TF_INLINE_INTERNAL
bool operator==(const frameref & lhs,   // friend of class frameref
                const frameref & rhs)
{ return lhs.p_ == rhs.p_ && lhs.o_ == rhs.o_; }

TF_INLINE_INTERNAL
bool operator!=(const frameref & lhs,   // friend of class frameref
                const frameref & rhs)
{ return lhs.p_ != rhs.p_ || lhs.o_ != rhs.o_; }

TF_INLINE_INTERNAL
frameref operator*(const transf & lhs,
                   const frameref & rhs)
{ return frameref(lhs * transf(rhs)); }

TF_INLINE_INTERNAL
transf operator/(const frameref & lhs,  // Safe; will not divide by zero
                 const frameref & rhs)
{ return transf(lhs)/transf(rhs); }

// ************************************************************************
// class frameref - Related global functions
// ************************************************************************

TF_INLINE_INTERNAL
void swap(frameref & f1, frameref & f2)
{ f1.swap(f2); }

TF_INLINE_INTERNAL
frameref applyto(const transf & thetransf,
                 const frameref & operand)
{ return thetransf.applyto(operand); }

TF_INLINE_INTERNAL
frameref interpolate(const frameref & a,
                     const frameref & b,
                     frameref::value_type t)
{ return (b/a).power(t) * a; }

// ************************************************************************
// class transf - Member functions involving frameref
// ************************************************************************

inline transf::transf(const frameref & f)  // explicit
   :v_(f.p_),r_(f.o_)
{}

inline frameref transf::applyto(const frameref & theframeref) const
{ return frameref(applyto(transf(theframeref))); }

// ************************************************************************
// End namespace tf
// ************************************************************************

}  // namespace tf

// ************************************************************************
// Internal-only macro undef's
// ************************************************************************

#undef TF_COS_INTERNAL
#undef TF_SIN_INTERNAL
#undef TF_ACOS_INTERNAL
#undef TF_SQRT_INTERNAL
#undef TF_PI_INTERNAL
#undef TF_INLINE_INTERNAL

#endif  //#ifndef TF_FILE_TRANSF_H_INCLUDED