$search
00001 /****************************************************************************** 00002 * \file 00003 * 00004 * $Id:$ 00005 * 00006 * Copyright (C) Brno University of Technology (BUT) 00007 * 00008 * This file is part of software developed by dcgm-robotics@FIT group. 00009 * 00010 * Author: Jan Gorig (xgorig01@stud.fit.vutbr.cz) 00011 * Supervised by: Michal Spanel (spanel@fit.vutbr.cz) 00012 * Date: 12/04/2012 00013 * 00014 * This file is free software: you can redistribute it and/or modify 00015 * it under the terms of the GNU Lesser General Public License as published by 00016 * the Free Software Foundation, either version 3 of the License, or 00017 * (at your option) any later version. 00018 * 00019 * This file is distributed in the hope that it will be useful, 00020 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00021 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00022 * GNU Lesser General Public License for more details. 00023 * 00024 * You should have received a copy of the GNU Lesser General Public License 00025 * along with this file. If not, see <http://www.gnu.org/licenses/>. 00026 */ 00027 00028 #include <srs_env_model/but_server/objtree/filter.h> 00029 00030 namespace objtree 00031 { 00032 00038 FilterBox::FilterBox(const Box &box) : 00039 m_box(box) 00040 { 00041 } 00042 00048 bool FilterBox::filter(const Box &dim) const 00049 { 00050 if(dim.x > m_box.x+m_box.w || dim.x+dim.w < m_box.x 00051 || dim.y > m_box.y+m_box.h || dim.y+dim.h < m_box.y 00052 || dim.z > m_box.z+m_box.d || dim.z+dim.d < m_box.z) 00053 { 00054 return false; 00055 } 00056 00057 return true; 00058 } 00059 00070 FilterPlane::FilterPlane(float posX, float posY, float posZ, float vecX, float vecY, float vecZ) 00071 { 00072 m_posX = posX; 00073 m_posY = posY; 00074 m_posZ = posZ; 00075 00076 m_vecX = vecX; 00077 m_vecY = vecY; 00078 m_vecZ = vecZ; 00079 } 00080 00086 bool FilterPlane::filter(const Box &dim) const 00087 { 00088 float pointX = dim.x, pointY = dim.y, pointZ = dim.z; 00089 00090 if(m_vecX >= 0.0f) pointX += dim.w; 00091 if(m_vecY >= 0.0f) pointY += dim.h; 00092 if(m_vecZ >= 0.0f) pointZ += dim.d; 00093 00094 float d = m_vecX*m_posX+m_vecY*m_posY+m_vecZ*m_posZ; 00095 00096 if(pointX*m_vecX+pointY*m_vecY+pointZ*m_vecZ > d) 00097 { 00098 return true; 00099 } 00100 00101 return false; 00102 } 00103 00109 bool FilterZero::filter(const Box &dim) const 00110 { 00111 return true; 00112 } 00113 00122 FilterSphere::FilterSphere(float x, float y, float z, float radius) : 00123 m_x(x), m_y(y), m_z(z), m_radiusSquare(radius*radius) 00124 { 00125 } 00126 00132 bool FilterSphere::filter(const Box &dim) const 00133 { 00134 float nearX, nearY, nearZ; 00135 00136 if(m_x < dim.x) nearX = dim.x; 00137 else if(m_x > dim.x+dim.w) nearX = dim.x+dim.w; 00138 else nearX = m_x; 00139 00140 if(m_y < dim.y) nearY = dim.y; 00141 else if(m_y > dim.y+dim.h) nearY = dim.y+dim.h; 00142 else nearY = m_y; 00143 00144 if(m_z < dim.z) nearZ = dim.z; 00145 else if(m_z > dim.z+dim.d) nearZ = dim.z+dim.d; 00146 else nearZ = m_z; 00147 00148 float xDist = m_x-nearX; 00149 float yDist = m_y-nearY; 00150 float zDist = m_z-nearZ; 00151 00152 if(xDist*xDist+yDist*yDist+zDist*zDist <= m_radiusSquare) 00153 { 00154 return true; 00155 } 00156 00157 00158 return false; 00159 } 00160 00161 }