base/OrientTest.cpp

Go to the documentation of this file.

/****************************************************************************
  Copyright (C)2002 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: OrientTest.cpp 1029 2004-02-11 20:45:54Z jungd $
  $Revision: 1.6 $
  $Date: 2004-02-11 15:45:54 -0500 (Wed, 11 Feb 2004) $
  $Author: jungd $
 
****************************************************************************/

#include <base/OrientTest>

#include <base/Math>

#include <base/Transform>

using base::OrientTest;

using base::Math;
using base::Quat4;
using base::Matrix;
using base::Vector;
using base::Transform;



OrientTest::OrientTest()
  : m(3,3)
{
}


void OrientTest::setUp() 
{ 

}
  

void OrientTest::tearDown()
{ 
}



void OrientTest::testQuatMat()
{
  // test a basic Quat4 rotation
  Quat4 qr(Vector3(1,1,0),consts::Pi/2.0);
  Point3 up(1,0,0);
  CPPUNIT_ASSERT( qr.rotate(up).equals(Point3(0.5,0.5,-0.707107),1e-5) );


  // test some basic interconversions first
  Quat4 rotx45(Vector3(1,0,0),Math::degToRad(45));
  Orient o(rotx45);

  CPPUNIT_ASSERT( o.getQuat4() == rotx45 );

  m.reset( o.getRotationMatrix() );
  CPPUNIT_ASSERT(m.size1() == 3);
  CPPUNIT_ASSERT(m.size2() == 3);

  Orient o2(m);

  Matrix m2( o2.getRotationMatrix() );
  
  CPPUNIT_ASSERT( base::equals(m2,m) );


  Quat4 q(Vector3(1,1,0),consts::Pi/4.0);

  // test two paths for getting the same matrix into an Orient
  Orient oq(q);
  Matrix4 oqm(oq.getRotationMatrix3()); 
  Orient  om = Matrix3(oqm);
  Matrix4 omm(om.getRotationMatrix3());
  CPPUNIT_ASSERT( omm.equals(oqm) );
 
  // test Quat4 & Orient to matrix conversions are identical
  Orient q3(q);
  Matrix3 m3(q3.getRotationMatrix3());
  CPPUNIT_ASSERT( Matrix4(m3).equals(Matrix4(q)) );
  

  // test quat->Mat->quat
  Orient q3m(m3);
  Orient q3mq(q3m.getQuat4());

  Quat4 q31(q3.getQuat4());
  Quat4 q32(q3mq.getQuat4());
  Point3 u(1,2,3);
  Point3 u1(q31.rotate(u));
  Point3 u2(q32.rotate(u));
  Point3 u3(m3*u);
  Point3 u4(q.rotate(u));

  CPPUNIT_ASSERT( u1.equals(u2) );
  CPPUNIT_ASSERT( u1.equals(u3) );
  CPPUNIT_ASSERT( u1.equals(u4) );
  
  // test quat inverse
  Quat4 q4(Vector3(1,2,4), consts::Pi/3.0);
  Quat4 q4i( inverse(q4) );
  CPPUNIT_ASSERT( q4i.rotate(q4.rotate(u)).equals(u) );
  Matrix4 m4(q4);
  Matrix4 m4i(q4i);
  CPPUNIT_ASSERT( (m4i*m4*u).equals(u) );
  Matrix4 m4id( m4i*m4 );
  Matrix4 I; I.setIdentity();
  CPPUNIT_ASSERT( m4id.equals(I) );

  // test orient inverse
  Orient o4 = Quat4(Vector3(1,2,4), consts::Pi/3.0);
  Orient o4i = inverse(o4.getQuat4());
  CPPUNIT_ASSERT( o4i.rotate(o4.rotate(u)).equals(u) );
  Matrix4 om4(o4);
  Matrix4 om4i(o4i);
  CPPUNIT_ASSERT( (om4i*om4*u).equals(u) );
  Matrix4 om4id( om4i*om4 );
  CPPUNIT_ASSERT( om4id.equals(I) );
}



void OrientTest::testTransform()
{
  Quat4 q(0.158724,0.508859,0.458874,0.710847);
  Orient o(q);
  Transform t(q);
  
  Point3 p(1,2,3);
  CPPUNIT_ASSERT( q.rotate(p).equals(o.rotate(p)) );
  CPPUNIT_ASSERT( q.rotate(p).equals(t.rotate(p)) );
}



void OrientTest::testEulerRPY()
{
  // construct a EulerRPY manually as rotations about
  //  axes and see if we get what we expect
  
  Real roll = consts::Pi/3.1;
  Real pitch = consts::Pi/5.1;
  Real yaw = consts::Pi/2.1;
  
  // construct rotation about X, Y, Z axes in that order (static/fixed frames)
  Transform rotx = Quat4(Vector3(1,0,0), roll);
  Transform roty = Quat4(Vector3(0,1,0), pitch);
  Transform rotz = Quat4(Vector3(0,0,1), yaw);
  Transform qt( rotz*roty*rotx );
  
  Orient rpyt(roll,pitch, yaw);
  
  Point3 p(1,2,3); // test point to transform
  Point3 pqt( qt.rotate(p) ); // transform using manually constructed quat
  Point3 prpyt( rpyt.rotate(p) ); // transform using RPY
  Point3 pit( rotz.rotate(roty.rotate(rotx.rotate(p))) ); // transform via sucessive application of individual rotations in order
  
  CPPUNIT_ASSERT( pqt.equals(prpyt) );
  CPPUNIT_ASSERT( pqt.equals(pit) );

  
  // convert to a Quat and back and test preservation
  Orient q(rpyt);
  q.changeRepresentation(Orient::Quat);
  CPPUNIT_ASSERT( q.rotate(p).equals(pqt) ); // check is same transformation
  q.changeRepresentation(Orient::EulerRPY);
  
  CPPUNIT_ASSERT( q.equals(rpyt) );

  // convert to matrix and back
  q.changeRepresentation(Orient::Mat);
  CPPUNIT_ASSERT( Point3(q.getRotationMatrix3() * p).equals(pqt) ); // check is same transformation
  q.changeRepresentation(Orient::EulerRPY);

  CPPUNIT_ASSERT( q.equals(rpyt) );
  
  // convert to EulerZYXs and back
  q.changeRepresentation(Orient::EulerZYXs);
  q.changeRepresentation(Orient::EulerRPY);
  CPPUNIT_ASSERT( q.equals(rpyt) );
  
  // Try converting from EulerRPY to Quat, to Mat and back
  Orient o1(1.57071,-1.00007,0.800039);
  Orient o2(o1); 
  o2.changeRepresentation(Orient::Quat);
  o2.changeRepresentation(Orient::Mat);
  o2.changeRepresentation(Orient::EulerRPY);
  CPPUNIT_ASSERT( o1.equals(o2) );
  
  // try another conversion from RPY->Quat and back at gymbal-lock config
  Orient o3(0,0,1.5708);
  Orient o4(o3);
  o3.changeRepresentation(Orient::Quat);
  o3.changeRepresentation(Orient::EulerRPY);
  CPPUNIT_ASSERT( o3.equals(o4) );
}





#ifdef DEBUG
CPPUNIT_TEST_SUITE_REGISTRATION( OrientTest );
#endif

---