base/Quat4.cpp

Go to the documentation of this file.

/****************************************************************************
  Copyright (C)1996 David Jung <opensim@pobox.com>

  This program/file is free software; you can redistribute it and/or modify
  it under the terms of the GNU General Public License as published by
  the Free Software Foundation; either version 2 of the License, or
  (at your option) any later version.
  
  This program 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 General Public License for more details. (http://www.gnu.org)
  
  You should have received a copy of the GNU General Public License
  along with this program; if not, write to the Free Software
  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  
  $Id: Quat4.cpp 1029 2004-02-11 20:45:54Z jungd $
  $Revision: 1.7 $
  $Date: 2004-02-11 15:45:54 -0500 (Wed, 11 Feb 2004) $
  $Author: jungd $
 
****************************************************************************/

#include <base/consts>
#include <base/Quat4>
#include <base/Matrix4>
#include <base/Math>
#include <base/Serializer>


using base::Quat4;
using base::Matrix4;
using base::Vector3;
using base::Point4;
using base::Math;



Quat4& Quat4::operator*=(const Quat4& qr)
{
  Quat4 ql(*this);

  w   = ql.w*qr.w   - ql.v.x*qr.v.x - ql.v.y*qr.v.y - ql.v.z*qr.v.z;
  v.x = ql.w*qr.v.x + ql.v.x*qr.w   + ql.v.y*qr.v.z - ql.v.z*qr.v.y;
  v.y = ql.w*qr.v.y + ql.v.y*qr.w   + ql.v.z*qr.v.x - ql.v.x*qr.v.z;
  v.z = ql.w*qr.v.z + ql.v.z*qr.w   + ql.v.x*qr.v.y - ql.v.y*qr.v.x;

  return *this;
}

void Quat4::setRotation(const Vector3& axis, Real angle)
{
  Vector3 a(axis);
  a.normalize();
  v=a*sin(angle/2.0);
  w=cos(angle/2.0);
}


void Quat4::setRotation(const Matrix4& rotation)
{
  // Algorithm is from the Matrix and Quaternion FAQ
  //  ( currently http://www.cs.ualberta.ca/~andreas/math/matrfaq_latest.html )
  const Matrix4& m = rotation;
  Quat4& q = *this;
  
  Real tr = 1 + m.e(1,1) + m.e(2,2) + m.e(3,3);
  if (tr > consts::epsilon) {
    Real s = sqrt(tr)*2.0;
    q.v.x = (m.e(3,2) - m.e(2,3))/s;
    q.v.y = (m.e(1,3) - m.e(3,1))/s;
    q.v.z = (m.e(2,1) - m.e(1,2))/s;
    q.w = 0.25*s;
  }     
  else {

    if ((m.e(1,1) > m.e(2,2)) && (m.e(1,1) > m.e(3,3))) {
      Real s = sqrt(1+m.e(1,1)-m.e(2,2)-m.e(3,3))*2.0;
      q.v.x = 0.25*s;
      q.v.y = (m.e(2,1)+m.e(1,2))/s;
      q.v.z = (m.e(1,3)+m.e(3,1))/s;
      q.w   = (m.e(3,2)-m.e(2,3))/s;
    }
    else if (m.e(2,2) > m.e(3,3)) {
      Real s = sqrt(1+m.e(2,2)-m.e(1,1)-m.e(3,3))*2.0;
      q.v.x = (m.e(2,1)+m.e(1,2))/s;
      q.v.y = 0.25*s;
      q.v.z = (m.e(3,2)+m.e(2,3))/s;
      q.w   = (m.e(1,3)-m.e(3,1))/s;
    }
    else {
      Real s = sqrt(1+m.e(3,3)-m.e(1,1)-m.e(2,2))*2.0;
      q.v.x = (m.e(1,3)+m.e(3,1))/s;
      q.v.y = (m.e(3,2)+m.e(2,3))/s;
      q.v.z = 0.25*s;
      q.w   = (m.e(2,1)-m.e(1,2))/s;
    }
  }
        
  //normalize();
}


void Quat4::getRotation(Vector3& axis, Real& angle) const
{
  angle = 2.0*acos(w);   /// !!!! this is just a guess, is it correct? (seems to work)
  if (!base::equals(angle,0))
    axis = v / sin(angle/2.0);
  else
    axis = Vector3(0,0,1);

  if (base::equals(axis.norm(),0))
    angle = 0;
}


void Quat4::rotatePoint(Point4& p) const
{
  const Quat4& q = *this;
  Quat4 r( (q * Quat4(p)) * inverse(q) );
  p=Point4(r.v.x,r.v.y,r.v.z,r.w);
}


/// Spherical Linear Interpolation
/// As t goes from 0 to 1, the Quat4 object goes from "from" to "to"
/// Reference: Shoemake at SIGGRAPH 89
/// See also
/// http://www.gamasutra.com/features/programming/19980703/quaternions_01.htm
Quat4 Quat4::interpolate(const Quat4& from, const Quat4& to, Real t)
{
  const Real linearTolerance = 1e-3;

  //  lifted from OSG code
  Real omega, cosomega, sinomega, scale_from, scale_to;

  Quat4 quatTo(to);

  // this is a dot product
  cosomega = from.v.dot(to.v)+(from.w*to.w);

  if ( cosomega <0.0 ) {
    cosomega = -cosomega;
    quatTo.negate();
  }

  if( (1.0 - cosomega) > linearTolerance ) {
    omega= Math::acos(cosomega) ;  // 0 <= omega <= Pi (see man acos)
    sinomega = Math::sin(omega) ;  // this sinomega should always be +ve so
    // could try sinomega=sqrt(1-cosomega*cosomega) to avoid a sin()?
    scale_from = Math::sin((1.0-t)*omega)/sinomega ;
    scale_to = Math::sin(t*omega)/sinomega ;
  }
  else {
    // The ends of the vectors are very close
    //  we can use simple linear interpolation - no need
    //   to worry about the "spherical" interpolation
    scale_from = 1.0 - t ;
    scale_to = t ;
  }
  
  Quat4 ret;
  ret.v = (from.v*scale_from) + (quatTo.v*scale_to);
  ret.w = (from.w*scale_from) + (quatTo.w*scale_to);
  return ret;
}


Real Quat4::angleBetween(const Quat4& q1, const Quat4& q2)
{
  // we calculate the angle a round-about way - there
  //  is probably a better one
  // transform a unit x-axis vector by each Quat and then use the dot product to get
  // and angle between them
  Vector3 xaxis(Vector3(1,0,0));  
  Vector3 vector1(q1.rotate(xaxis));
  Vector3 vector2(q2.rotate(xaxis));
  return Math::acos(dot(vector1,vector2));
}

        

Quat4::operator Matrix4() const
{
  Real nq = norm();
  Real s = (nq> Real(0))? (Real(2)/base::sqrt(nq)):Real(0);

  Real xs = v.x*s, ys = v.y*s, zs = v.z*s;
  Real wx = w*xs,  wy = w*ys,  wz = w*zs;
  Real xx = v.x*xs,xy = v.x*ys,xz = v.x*zs;
  Real yy = v.y*ys,yz = v.y*zs,zz = v.z*zs;

  Matrix4 m; // identity
        
  m.e(1,1) = Real(1) - (yy+zz);  m.e(2,1) = xy+wz;             m.e(3,1) = xz-wy;
  m.e(1,2) = xy - wz;            m.e(2,2) = Real(1) - (xx+zz); m.e(3,2) = yz+wx;
  m.e(1,3) = xz+wy;              m.e(2,3) = yz-wx;             m.e(3,3) = Real(1) - (xx+yy);

  return m;
}


void Quat4::serialize(Serializer& s)
{
  s(v,"v"); s(w,"w");
}



// output
std::ostream& base::operator<<(std::ostream& out, const Quat4& q)
{
  return out << "([" << q.v.x << "," << q.v.y << "," << q.v.z << "]," << q.w << ")";
}

---