plane.cpp
Go to the documentation of this file.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 <cstdio> 00029 #include <cmath> 00030 #include <srs_env_model/but_server/objtree/plane.h> 00031 00032 namespace objtree 00033 { 00034 00040 Plane::Plane(const Polygon &points) : 00041 m_points(points) 00042 { 00043 m_type = PLANE; 00044 00045 m_pos = points[0]; 00046 00047 Vector vec1(points[0]-points[1]); 00048 Vector vec2(points[0]-points[2]); 00049 00050 m_normal.x = vec1.y*vec2.z-vec1.z*vec2.y; 00051 m_normal.y = vec1.z*vec2.x-vec1.x*vec2.z; 00052 m_normal.z = vec1.x*vec2.y-vec1.y*vec2.x; 00053 00054 float vecSize = sqrt(m_normal.x*m_normal.x + m_normal.y*m_normal.y + m_normal.z*m_normal.z); 00055 m_normal.x /= vecSize; 00056 m_normal.y /= vecSize; 00057 m_normal.z /= vecSize; 00058 00059 m_d = - m_normal.x*m_pos.x - m_normal.y*m_pos.y - m_normal.z*m_pos.z; 00060 00061 m_boundingMin = m_boundingMax = points[0]; 00062 00063 for(Polygon::const_iterator i = points.begin(); i != points.end(); i++) 00064 { 00065 if(i->x < m_boundingMin.x) m_boundingMin.x = i->x; 00066 if(i->y < m_boundingMin.y) m_boundingMin.y = i->y; 00067 if(i->z < m_boundingMin.z) m_boundingMin.z = i->z; 00068 00069 if(i->x > m_boundingMax.x) m_boundingMax.x = i->x; 00070 if(i->y > m_boundingMax.y) m_boundingMax.y = i->y; 00071 if(i->z > m_boundingMax.z) m_boundingMax.z = i->z; 00072 } 00073 } 00074 00082 Plane::Plane(const Point ¢er, const Vector &normal, const Point &scale) 00083 { 00084 m_type = PLANE; 00085 00086 m_pos = center; 00087 m_normal = normal; 00088 00089 float vecSize = sqrt(m_normal.x*m_normal.x + m_normal.y*m_normal.y + m_normal.z*m_normal.z); 00090 m_normal.x /= vecSize; 00091 m_normal.y /= vecSize; 00092 m_normal.z /= vecSize; 00093 00094 m_d = - m_normal.x*m_pos.x - m_normal.y*m_pos.y - m_normal.z*m_pos.z; 00095 00096 m_boundingMin = center - scale/2.0f; 00097 m_boundingMax = center + scale/2.0f; 00098 00099 //TODO: points 00100 } 00101 00108 bool Plane::fitsIntoBox(const Box &box) const 00109 { 00110 return false; 00111 } 00112 00118 bool Plane::interfereWithBox(const Box &box) const 00119 { 00120 Point min(box.x, box.y, box.z); 00121 Point max(min); 00122 00123 if(m_normal.x >= 0.0f) max.x += box.w; 00124 else min.x += box.w; 00125 if(m_normal.y >= 0.0f) max.y += box.h; 00126 else min.y += box.h; 00127 if(m_normal.z >= 0.0f) max.z += box.d; 00128 else min.z += box.d; 00129 00130 if(min.x*m_normal.x+min.y*m_normal.y+min.z*m_normal.z > -m_d) 00131 return false; 00132 00133 if(max.x*m_normal.x+max.y*m_normal.y+max.z*m_normal.z < -m_d) 00134 return false; 00135 00136 if(m_boundingMin.x > box.x+box.w || m_boundingMin.y > box.y+box.h || m_boundingMin.z > box.z+box.d) 00137 return false; 00138 00139 if(m_boundingMax.x < box.x || m_boundingMax.y < box.y || m_boundingMax.z < box.z) 00140 return false; 00141 00142 return true; 00143 } 00144 00150 bool Plane::isSimilar(const Object *object) const 00151 { 00152 if(object->type() != PLANE) return false; 00153 00154 Plane *plane = (Plane*)object; 00155 00156 //Normals must be normalized! 00157 00158 //Dot product is lesser than 0.3 => angle is too big => false 00159 if(fabs(m_normal.x*plane->m_normal.x+m_normal.y*plane->m_normal.y+m_normal.z*plane->m_normal.z) < 0.3f) return false; 00160 00161 //Distance between point and plane 00162 const Point &p = plane->m_pos; 00163 if(fabs(p.x*m_normal.x+p.y*m_normal.y+p.z*m_normal.z+m_d) > 0.5f) return false; 00164 00165 return true; 00166 } 00167 00175 bool Plane::isPointInside(float x, float y, float z) const 00176 { 00177 return fabs(x*m_normal.x+y*m_normal.y+z*m_normal.z+m_d) < 0.1f; 00178 } 00179 00180 }