$search
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 }