uwsim: GPSSensor.cpp Source File

Go to the documentation of this file.
00001 /* 
00002  * Copyright (c) 2013 University of Jaume-I.
00003  * All rights reserved. This program and the accompanying materials
00004  * are made available under the terms of the GNU Public License v3.0
00005  * which accompanies this distribution, and is available at
00006  * http://www.gnu.org/licenses/gpl.html
00007  * 
00008  * Contributors:
00009  *     Mario Prats
00010  *     Javier Perez
00011  */
00012 
00013 #include <uwsim/UWSimUtils.h>
00014 #include <uwsim/GPSSensor.h>
00015 #include <osg/io_utils>
00016 
00017 osg::Vec3d GPSSensor::getMeasurement()
00018 {
00019   //Should get world coords and then transform to the localizedWorld
00020   boost::shared_ptr<osg::Matrix> rMs = getWorldCoords(node_);
00021   osg::Matrixd lMs = *rMs * osg::Matrixd::inverse(rMl_);
00022 
00023   //Now add some gaussian noise
00024   static boost::normal_distribution<> normal(0, std_);
00025   static boost::variate_generator<boost::mt19937&, boost::normal_distribution<> > var_nor(rng_, normal);
00026 
00027   return lMs.getTrans() + osg::Vec3d(var_nor(), var_nor(), var_nor());
00028 }
00029 
00030 double GPSSensor::depthBelowWater()
00031 {
00032   boost::shared_ptr<osg::Matrix> rMs = getWorldCoords(node_);
00033   return -(rMs->getTrans().z() - oscene_->getOceanScene()->getOceanSurfaceHeight());
00034 }