ogre_tools: utest.cpp Source File

00001 /*
00002  * Copyright (c) 2008, Willow Garage, Inc.
00003  * All rights reserved.
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  *     * Redistributions of source code must retain the above copyright
00009  *       notice, this list of conditions and the following disclaimer.
00010  *     * Redistributions in binary form must reproduce the above copyright
00011  *       notice, this list of conditions and the following disclaimer in the
00012  *       documentation and/or other materials provided with the distribution.
00013  *     * Neither the name of the Willow Garage, Inc. nor the names of its
00014  *       contributors may be used to endorse or promote products derived from
00015  *       this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 /* Author: Josh Faust */
00031 
00032 #include <gtest/gtest.h>
00033 #include "../src/ogre_tools/fps_camera.h"
00034 #include "../src/ogre_tools/orbit_camera.h"
00035 #include "../src/ogre_tools/initialization.h"
00036 
00037 #include <ros/package.h>
00038 
00039 #include <OGRE/Ogre.h>
00040 
00041 bool g_initialized = false;
00042 Ogre::Root* g_root = NULL;
00043 Ogre::SceneManager* g_scene_manager = NULL;
00044 Ogre::Viewport* g_viewport = NULL;
00045 Ogre::RenderWindow* g_render_window = NULL;
00046 
00047 using namespace ogre_tools;
00048 
00049 class TestEnvironment : public testing::Environment
00050 {
00051 public:
00052   virtual void SetUp()
00053   {
00054     Ogre::LogManager* log_manager = new Ogre::LogManager();
00055     log_manager->createLog( "Ogre.log", false, false, true );
00056 
00057     g_root = new Ogre::Root();
00058     g_root->loadPlugin( "RenderSystem_GL" );
00059     g_root->loadPlugin( "Plugin_OctreeSceneManager" );
00060 
00061     std::string ogre_tools_path = ros::package::getPath(ROS_PACKAGE_NAME);
00062     Ogre::ResourceGroupManager::getSingleton().addResourceLocation( ogre_tools_path + "/media", "FileSystem", ROS_PACKAGE_NAME );
00063     Ogre::ResourceGroupManager::getSingleton().addResourceLocation( ogre_tools_path + "/media/models", "FileSystem", ROS_PACKAGE_NAME );
00064     ogre_tools::initializeResources(ogre_tools::V_string());
00065 
00066     // Taken from gazebo
00067     const Ogre::RenderSystemList *rsList = g_root->getAvailableRenderers();
00068 
00069     Ogre::RenderSystem* render_system = NULL;
00070     Ogre::RenderSystemList::const_iterator renderIt = rsList->begin();
00071     Ogre::RenderSystemList::const_iterator renderEnd = rsList->end();
00072     for ( ; renderIt != renderEnd; ++renderIt )
00073     {
00074       render_system = *renderIt;
00075 
00076       if ( render_system->getName() == "OpenGL Rendering Subsystem" )
00077       {
00078         break;
00079       }
00080     }
00081 
00082     if ( render_system == NULL )
00083     {
00084       printf( "Could not find the opengl rendering subsystem!\n" );
00085       exit(1);
00086     }
00087 
00088     render_system->setConfigOption("Full Screen","No");
00089     render_system->setConfigOption("FSAA","2");
00090     render_system->setConfigOption("RTT Preferred Mode", "PBuffer");
00091 
00092     g_root->setRenderSystem( render_system );
00093     g_render_window = g_root->initialise( true );
00094     g_scene_manager = g_root->createSceneManager( Ogre::ST_GENERIC, "TestSceneManager" );
00095     g_viewport = g_render_window->addViewport( NULL );
00096 
00097     g_root->renderOneFrame();
00098   }
00099 
00100   virtual void TearDown()
00101   {
00102     g_scene_manager->clearScene();
00103     g_root->destroySceneManager( g_scene_manager );
00104 
00105     delete g_root;
00106   }
00107 };
00108 
00109 #define EXPECT_VECTOR_EQ( expected, actual ) \
00110   EXPECT_FLOAT_EQ( expected.x, actual.x ); \
00111   EXPECT_FLOAT_EQ( expected.y, actual.y ); \
00112   EXPECT_FLOAT_EQ( expected.z, actual.z );
00113 
00114 #define EXPECT_VECTOR_NEAR( expected, actual, tolerance ) \
00115   EXPECT_NEAR( expected.x, actual.x, tolerance ); \
00116   EXPECT_NEAR( expected.y, actual.y, tolerance ); \
00117   EXPECT_NEAR( expected.z, actual.z, tolerance );
00118 
00119 #define EXPECT_QUATERNION_EQ( expected, actual ) \
00120   EXPECT_FLOAT_EQ( expected.x, actual.x ); \
00121   EXPECT_FLOAT_EQ( expected.y, actual.y ); \
00122   EXPECT_FLOAT_EQ( expected.z, actual.z ); \
00123   EXPECT_FLOAT_EQ( expected.w, actual.w );
00124 
00125 #define EXPECT_QUATERNION_NEAR( expected, actual, tolerance ) \
00126   EXPECT_NEAR( expected.x, actual.x, tolerance ); \
00127   EXPECT_NEAR( expected.y, actual.y, tolerance ); \
00128   EXPECT_NEAR( expected.z, actual.z, tolerance ); \
00129   EXPECT_NEAR( expected.w, actual.w, tolerance );
00130 
00131 TEST(OrbitCamera, setPosition)
00132 {
00133   OrbitCamera* orbit_cam = new OrbitCamera( g_scene_manager );
00134   g_viewport->setCamera( orbit_cam->getOgreCamera() );
00135 
00136   Ogre::Vector3 expected_position( 10.0f, 442.0f, -194.0f );
00137   orbit_cam->CameraBase::setPosition( expected_position );
00138   Ogre::Vector3 actual_position = orbit_cam->getPosition();
00139 
00140   EXPECT_VECTOR_EQ( expected_position, actual_position );
00141 
00142   g_viewport->setCamera( NULL );
00143 
00144   delete orbit_cam;
00145 }
00146 
00147 TEST(OrbitCamera, setOrientation)
00148 {
00149   OrbitCamera* orbit_cam = new OrbitCamera( g_scene_manager );
00150   g_viewport->setCamera( orbit_cam->getOgreCamera() );
00151 
00152   Ogre::Quaternion expected_orientation( Ogre::Radian( Ogre::Math::HALF_PI / 2.0f ), Ogre::Vector3( 0.0f, 1.0f, 0.0f ) );
00153   expected_orientation = expected_orientation * Ogre::Quaternion( Ogre::Radian( Ogre::Math::HALF_PI / 2.0f ), Ogre::Vector3( 1.0f, 0.0f, 0.0f ) );
00154 
00155   orbit_cam->CameraBase::setOrientation( expected_orientation );
00156 
00157   Ogre::Quaternion actual_orientation = orbit_cam->getOrientation();
00158 
00159   EXPECT_QUATERNION_NEAR( expected_orientation, actual_orientation, 0.000001 );
00160 
00161   g_viewport->setCamera( NULL );
00162 
00163   delete orbit_cam;
00164 }
00165 
00166 TEST(OrbitCamera, saveAndLoad)
00167 {
00168   OrbitCamera* cam = new OrbitCamera( g_scene_manager );
00169   g_viewport->setCamera( cam->getOgreCamera() );
00170 
00171   const Ogre::Vector3 pos = Ogre::Vector3(0.0f, 10.0f, 0.0f);
00172   const Ogre::Vector3 fp = Ogre::Vector3(0.0f, 5.0f, 0.0f);
00173   cam->setFocalPoint(fp);
00174   cam->CameraBase::setPosition(pos);
00175   float pitch = cam->getPitch();
00176   float yaw = cam->getYaw();
00177   float distance = cam->getDistance();
00178 
00179   std::string str = cam->toString();
00180   cam->fromString(str);
00181 
00182   EXPECT_NEAR(pitch, cam->getPitch(), 0.001);
00183   EXPECT_NEAR(yaw, cam->getYaw(), 0.001);
00184   EXPECT_NEAR(distance, cam->getDistance(), 0.001);
00185   EXPECT_VECTOR_NEAR(pos, cam->getPosition(), 0.01);
00186   EXPECT_VECTOR_NEAR(fp, cam->getFocalPoint(), 0.001);
00187 
00188   g_viewport->setCamera( NULL );
00189 
00190   delete cam;
00191 }
00192 
00193 TEST(FPSCamera, setPosition)
00194 {
00195   FPSCamera* fps_cam = new FPSCamera( g_scene_manager );
00196   g_viewport->setCamera( fps_cam->getOgreCamera() );
00197 
00198   Ogre::Vector3 expected_position( 10.0f, 442.0f, -194.0f );
00199   fps_cam->CameraBase::setPosition( expected_position );
00200   Ogre::Vector3 actual_position = fps_cam->getPosition();
00201 
00202   EXPECT_VECTOR_EQ( expected_position, actual_position );
00203 
00204   g_viewport->setCamera( NULL );
00205 
00206   delete fps_cam;
00207 }
00208 
00209 TEST(FPSCamera, setOrientation)
00210 {
00211   FPSCamera* fps_cam = new FPSCamera( g_scene_manager );
00212   g_viewport->setCamera( fps_cam->getOgreCamera() );
00213 
00214   Ogre::Quaternion expected_orientation( Ogre::Radian( Ogre::Math::HALF_PI / 2.0f ), Ogre::Vector3( 0.0f, 1.0f, 0.0f ) );
00215   expected_orientation = expected_orientation * Ogre::Quaternion( Ogre::Radian( Ogre::Math::HALF_PI / 2.0f ), Ogre::Vector3( 1.0f, 0.0f, 0.0f ) );
00216 
00217   fps_cam->CameraBase::setOrientation( expected_orientation );
00218 
00219   Ogre::Quaternion actual_orientation = fps_cam->getOrientation();
00220 
00221   EXPECT_QUATERNION_NEAR( expected_orientation, actual_orientation, 0.000001 );
00222 
00223   g_viewport->setCamera( NULL );
00224 
00225   delete fps_cam;
00226 }
00227 
00228 TEST(FPSCamera, saveAndLoad)
00229 {
00230   FPSCamera* cam = new FPSCamera( g_scene_manager );
00231   g_viewport->setCamera( cam->getOgreCamera() );
00232 
00233   const Ogre::Vector3 pos = Ogre::Vector3(0.0f, 10.0f, 0.0f);
00234   const Ogre::Vector3 fp = Ogre::Vector3(0.0f, 5.0f, 0.0f);
00235   cam->lookAt(fp);
00236   cam->CameraBase::setPosition(pos);
00237   float pitch = cam->getPitch();
00238   float yaw = cam->getYaw();
00239 
00240   std::string str = cam->toString();
00241   cam->fromString(str);
00242 
00243   EXPECT_NEAR(pitch, cam->getPitch(), 0.001);
00244   EXPECT_NEAR(yaw, cam->getYaw(), 0.001);
00245   EXPECT_VECTOR_NEAR(pos, cam->getPosition(), 0.001);
00246 
00247   g_viewport->setCamera( NULL );
00248 
00249   delete cam;
00250 }
00251 
00252 class OrbitSetFromFPS : public testing::Test
00253 {
00254 public:
00255   virtual void SetUp()
00256   {
00257     fps_cam_ = new FPSCamera( g_scene_manager );
00258     orbit_cam_ = new OrbitCamera( g_scene_manager );
00259   }
00260 
00261   virtual void TearDown()
00262   {
00263     delete fps_cam_;
00264     delete orbit_cam_;
00265   }
00266 
00267   void TestQuadrant( float x, float y, float z )
00268   {
00269     fps_cam_->CameraBase::setPosition( Ogre::Vector3( x, y, z ) );
00270     fps_cam_->CameraBase::setOrientation( Ogre::Quaternion( Ogre::Radian( Ogre::Math::HALF_PI ), Ogre::Vector3::UNIT_X ) );
00271 
00272     Ogre::Vector3 expected_position = fps_cam_->getPosition();
00273     Ogre::Quaternion expected_orientation = fps_cam_->getOrientation();
00274 
00275     orbit_cam_->setFrom( fps_cam_ );
00276 
00277     Ogre::Vector3 actual_position = orbit_cam_->getPosition();
00278     Ogre::Quaternion actual_orientation = orbit_cam_->getOrientation();
00279 
00280     EXPECT_VECTOR_NEAR( expected_position, actual_position, 0.001 );
00281     EXPECT_QUATERNION_NEAR( expected_orientation, actual_orientation, 0.001 );
00282   }
00283 
00284   FPSCamera* fps_cam_;
00285   OrbitCamera* orbit_cam_;
00286 };
00287 
00288 TEST_F(OrbitSetFromFPS, quadrantPosXPosYPosZ)
00289 {
00290   TestQuadrant( 1.0f, 1.0f, 1.0f );
00291 }
00292 
00293 TEST_F(OrbitSetFromFPS, quadrantPosXNegYPosZ)
00294 {
00295   TestQuadrant( 1.0f, -1.0f, 1.0f );
00296 }
00297 
00298 TEST_F(OrbitSetFromFPS, quadrantNegXPosYPosZ)
00299 {
00300   TestQuadrant( -1.0f, 1.0f, 1.0f );
00301 }
00302 
00303 TEST_F(OrbitSetFromFPS, quadrantNegXNegYPosZ)
00304 {
00305   TestQuadrant( -1.0f, -1.0f, 1.0f );
00306 }
00307 
00308 TEST_F(OrbitSetFromFPS, quadrantNegXPosYNegZ)
00309 {
00310   TestQuadrant( -1.0f, 1.0f, -1.0f );
00311 }
00312 
00313 TEST_F(OrbitSetFromFPS, quadrantNegXNegYNegZ)
00314 {
00315   TestQuadrant( -1.0f, -1.0f, -1.0f );
00316 }
00317 
00318 TEST_F(OrbitSetFromFPS, quadrantPosXPosYNegZ)
00319 {
00320   TestQuadrant( 1.0f, 1.0f, -1.0f );
00321 }
00322 
00323 TEST_F(OrbitSetFromFPS, quadrantPosXNegYNegZ)
00324 {
00325   TestQuadrant( 1.0f, -1.0f, -1.0f );
00326 }
00327 
00328 class FPSSetFromOrbit : public testing::Test
00329 {
00330 public:
00331   virtual void SetUp()
00332   {
00333     fps_cam_ = new FPSCamera( g_scene_manager );
00334     orbit_cam_ = new OrbitCamera( g_scene_manager );
00335   }
00336 
00337   virtual void TearDown()
00338   {
00339     delete fps_cam_;
00340     delete orbit_cam_;
00341   }
00342 
00343   void TestQuadrant( float x, float y, float z )
00344   {
00345     orbit_cam_->CameraBase::setPosition( Ogre::Vector3( x, y, z ) );
00346 
00347     Ogre::Vector3 expected_position = orbit_cam_->getPosition();
00348     Ogre::Quaternion expected_orientation = orbit_cam_->getOrientation();
00349 
00350     fps_cam_->setFrom( orbit_cam_ );
00351 
00352     Ogre::Vector3 actual_position = fps_cam_->getPosition();
00353     Ogre::Quaternion actual_orientation = fps_cam_->getOrientation();
00354 
00355     EXPECT_VECTOR_NEAR( expected_position, actual_position, 0.001 );
00356     EXPECT_QUATERNION_NEAR( expected_orientation, actual_orientation, 0.001 );
00357   }
00358 
00359   FPSCamera* fps_cam_;
00360   OrbitCamera* orbit_cam_;
00361 };
00362 
00363 TEST_F(FPSSetFromOrbit, quadrantPosXPosYPosZ)
00364 {
00365   TestQuadrant( 1.0f, 1.0f, 1.0f );
00366 }
00367 
00368 TEST_F(FPSSetFromOrbit, quadrantPosXNegYPosZ)
00369 {
00370   TestQuadrant( 1.0f, -1.0f, 1.0f );
00371 }
00372 
00373 TEST_F(FPSSetFromOrbit, quadrantNegXPosYPosZ)
00374 {
00375   TestQuadrant( -1.0f, 1.0f, 1.0f );
00376 }
00377 
00378 TEST_F(FPSSetFromOrbit, quadrantNegXNegYPosZ)
00379 {
00380   TestQuadrant( -1.0f, -1.0f, 1.0f );
00381 }
00382 
00383 TEST_F(FPSSetFromOrbit, quadrantNegXPosYNegZ)
00384 {
00385   TestQuadrant( -1.0f, 1.0f, -1.0f );
00386 }
00387 
00388 TEST_F(FPSSetFromOrbit, quadrantNegXNegYNegZ)
00389 {
00390   TestQuadrant( -1.0f, -1.0f, -1.0f );
00391 }
00392 
00393 TEST_F(FPSSetFromOrbit, quadrantPosXPosYNegZ)
00394 {
00395   TestQuadrant( 1.0f, 1.0f, -1.0f );
00396 }
00397 
00398 TEST_F(FPSSetFromOrbit, quadrantPosXNegYNegZ)
00399 {
00400   TestQuadrant( 1.0f, -1.0f, -1.0f );
00401 }
00402 
00403 int main( int argc, char** argv )
00404 {
00405   testing::InitGoogleTest( &argc, argv );
00406   testing::AddGlobalTestEnvironment( new TestEnvironment );
00407 
00408   return RUN_ALL_TESTS();
00409 }