1 /****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 by Inria. All rights reserved.
5 *
6 * This software is free software; you can redistribute it and/or modify
7 * it under the terms of the GNU General Public License as published by
8 * the Free Software Foundation; either version 2 of the License, or
9 * (at your option) any later version.
10 * See the file LICENSE.txt at the root directory of this source
11 * distribution for additional information about the GNU GPL.
12 *
13 * For using ViSP with software that can not be combined with the GNU
14 * GPL, please contact Inria about acquiring a ViSP Professional
15 * Edition License.
16 *
17 * See http://visp.inria.fr for more information.
18 *
19 * This software was developed at:
20 * Inria Rennes - Bretagne Atlantique
21 * Campus Universitaire de Beaulieu
22 * 35042 Rennes Cedex
23 * France
24 *
25 * If you have questions regarding the use of this file, please contact
26 * Inria at visp@inria.fr
27 *
28 * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29 * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30 *
31 * Description:
32 * Defines a (possibly oriented) rectangle in the plane.
33 *
34 * Author:
35 * Pierre Chatelain
36 * Marc Pouliquen
37 *
38 *****************************************************************************/
39 #include <visp3/core/vpRectOriented.h>
40
41 #include <cmath>
42
43 /// Default constructor.
vpRectOriented()44 vpRectOriented::vpRectOriented()
45 : m_center(), m_width(), m_height(), m_theta(), m_topLeft(), m_topRight(), m_bottomLeft(), m_bottomRight()
46 {
47 }
48
49 /** Constructor.
50 * @param center The rectangle center.
51 * @param width The rectangle width.
52 * @param height The rectangle height.
53 * @param theta The rectangle orientation (rad).
54 */
vpRectOriented(const vpImagePoint & center,double width,double height,double theta)55 vpRectOriented::vpRectOriented(const vpImagePoint ¢er, double width, double height, double theta)
56 {
57 m_center = center;
58 m_width = width;
59 m_height = height;
60 m_theta = theta;
61 m_topLeft.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2.0);
62 m_topLeft.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2.0);
63 m_bottomLeft.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2.0);
64 m_bottomLeft.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2.0);
65 m_bottomRight.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2.0);
66 m_bottomRight.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2.0);
67 m_topRight.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2.0);
68 m_topRight.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2.0);
69 }
70
71 /** Copy constructor.
72 * @param rect Rectangle to copy.
73 */
vpRectOriented(const vpRect & rect)74 vpRectOriented::vpRectOriented(const vpRect &rect)
75 {
76 m_center = rect.getCenter();
77 m_width = rect.getWidth();
78 m_height = rect.getHeight();
79 m_theta = .0;
80 m_topLeft.set_i(m_center.get_i() - m_height / 2.0);
81 m_topLeft.set_j(m_center.get_j() - m_width / 2.0);
82 m_bottomLeft.set_i(m_center.get_i() + m_height / 2.0);
83 m_bottomLeft.set_j(m_center.get_j() - m_width / 2.0);
84 m_bottomRight.set_i(m_center.get_i() + m_height / 2.0);
85 m_bottomRight.set_j(m_center.get_j() + m_width / 2.0);
86 m_topRight.set_i(m_center.get_i() - m_height / 2.0);
87 m_topRight.set_j(m_center.get_j() + m_width / 2.0);
88 }
89
90 #if (VISP_CXX_STANDARD < VISP_CXX_STANDARD_11)
91 /** Assignement operator.
92 * @param rectOriented Oriented rectangle to copy.
93 */
operator =(const vpRectOriented & rectOriented)94 vpRectOriented &vpRectOriented::operator=(const vpRectOriented &rectOriented)
95 {
96 m_center = rectOriented.getCenter();
97 m_width = rectOriented.getWidth();
98 m_height = rectOriented.getHeight();
99 m_theta = rectOriented.getOrientation();
100 m_topLeft = rectOriented.getTopLeft();
101 m_bottomLeft = rectOriented.getBottomLeft();
102 m_bottomRight = rectOriented.getBottomRight();
103 m_topRight = rectOriented.getTopRight();
104 return *this;
105 }
106 #endif
107
108 /** Assignement operator from vpRect.
109 * @param rect Rectangle to copy.
110 */
operator =(const vpRect & rect)111 vpRectOriented &vpRectOriented::operator=(const vpRect &rect)
112 {
113 m_center = rect.getCenter();
114 m_width = rect.getWidth();
115 m_height = rect.getHeight();
116 m_theta = .0;
117 m_topLeft.set_i(m_center.get_i() - m_height / 2.0);
118 m_topLeft.set_j(m_center.get_j() - m_width / 2.0);
119 m_bottomLeft.set_i(m_center.get_i() + m_height / 2.0);
120 m_bottomLeft.set_j(m_center.get_j() - m_width / 2.0);
121 m_bottomRight.set_i(m_center.get_i() + m_height / 2.0);
122 m_bottomRight.set_j(m_center.get_j() + m_width / 2.0);
123 m_topRight.set_i(m_center.get_i() - m_height / 2.0);
124 m_topRight.set_j(m_center.get_j() + m_width / 2.0);
125 return *this;
126 }
127
128 /** Conversion to vpRect operator.
129 */
operator vpRect()130 vpRectOriented::operator vpRect()
131 {
132 if (std::fabs(m_theta) > std::numeric_limits<double>::epsilon())
133 throw(vpException(vpException::badValue, "Cannot convert a vpRectOriented with non-zero orientation to a vpRect"));
134
135 return vpRect(m_topLeft, m_bottomRight);
136 }
137
138 /** Set the corners of the rectangle.
139 * @warning This method doesn't check whether the 4 points actually form a rectangle!
140 * The behaviour is undefined if it is not the case.
141 */
setPoints(const vpImagePoint & topLeft,const vpImagePoint & topRight,const vpImagePoint & bottomLeft,const vpImagePoint & bottomRight)142 void vpRectOriented::setPoints(const vpImagePoint &topLeft, const vpImagePoint &topRight,
143 const vpImagePoint &bottomLeft, const vpImagePoint &bottomRight)
144 {
145 m_topLeft = topLeft;
146 m_bottomLeft = bottomLeft;
147 m_bottomRight = bottomRight;
148 m_topRight = topRight;
149 m_center.set_i((m_topLeft.get_i() + m_bottomLeft.get_i() + m_bottomRight.get_i() + m_topRight.get_i()) / 4.0);
150 m_center.set_j((m_topLeft.get_j() + m_bottomLeft.get_j() + m_bottomRight.get_j() + m_topRight.get_j()) / 4.0);
151 m_width = sqrt((m_topRight.get_i() - m_topLeft.get_i()) * (m_topRight.get_i() - m_topLeft.get_i()) +
152 (m_topRight.get_j() - m_topLeft.get_j()) * (m_topRight.get_j() - m_topLeft.get_j()));
153 m_height = sqrt((m_bottomLeft.get_i() - m_topLeft.get_i()) * (m_bottomLeft.get_i() - m_topLeft.get_i()) +
154 (m_bottomLeft.get_j() - m_topLeft.get_j()) * (m_bottomLeft.get_j() - m_topLeft.get_j()));
155 m_theta = atan2(m_topRight.get_i() - m_topLeft.get_i(), m_topRight.get_j() - m_topLeft.get_j());
156 }
157
158 /// Set the center of the rectangle.
setCenter(const vpImagePoint & center)159 void vpRectOriented::setCenter(const vpImagePoint ¢er)
160 {
161 m_topLeft += center - m_center;
162 m_bottomLeft += center - m_center;
163 m_bottomRight += center - m_center;
164 m_topRight += center - m_center;
165 m_center = center;
166 }
167
168 /// Get the rectangle center point.
getCenter() const169 vpImagePoint vpRectOriented::getCenter() const { return m_center; }
170
171 /// Get the top-left corner.
getTopLeft() const172 vpImagePoint vpRectOriented::getTopLeft() const { return m_topLeft; }
173
174 /// Get the top-right corner.
getTopRight() const175 vpImagePoint vpRectOriented::getTopRight() const { return m_topRight; }
176
177 /// Get the bottom-left corner.
getBottomLeft() const178 vpImagePoint vpRectOriented::getBottomLeft() const { return m_bottomLeft; }
179
180 /// Get the bottom-right corner.
getBottomRight() const181 vpImagePoint vpRectOriented::getBottomRight() const { return m_bottomRight; }
182
183 /// Set the size of the rectangle : performs a homothety relatively to the rectangle center.
setSize(double width,double height)184 void vpRectOriented::setSize(double width, double height)
185 {
186 m_width = width;
187 m_height = height;
188 m_topLeft.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2);
189 m_topLeft.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2);
190 m_bottomLeft.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2);
191 m_bottomLeft.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2);
192 m_bottomRight.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2);
193 m_bottomRight.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2);
194 m_topRight.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2);
195 m_topRight.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2);
196 }
197
198 /// Get the rectangle width.
getWidth() const199 double vpRectOriented::getWidth() const { return m_width; }
200
201 /// Get the rectangle height.
getHeight() const202 double vpRectOriented::getHeight() const { return m_height; }
203
204 /// Set the rectangle orientation (rad).
setOrientation(double theta)205 void vpRectOriented::setOrientation(double theta)
206 {
207 m_theta = theta;
208 m_topLeft.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2);
209 m_topLeft.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2);
210 m_bottomLeft.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 - m_width * sin(m_theta) / 2);
211 m_bottomLeft.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 - m_width * cos(m_theta) / 2);
212 m_bottomRight.set_i(m_center.get_i() + m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2);
213 m_bottomRight.set_j(m_center.get_j() - m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2);
214 m_topRight.set_i(m_center.get_i() - m_height * cos(m_theta) / 2.0 + m_width * sin(m_theta) / 2);
215 m_topRight.set_j(m_center.get_j() + m_height * sin(m_theta) / 2.0 + m_width * cos(m_theta) / 2);
216 }
217
218 /// Get the rectangle orientation (rad).
getOrientation() const219 double vpRectOriented::getOrientation() const { return m_theta; }
220
221 /// Check whether the point is inside the rectangle.
isInside(const vpImagePoint & point) const222 bool vpRectOriented::isInside(const vpImagePoint &point) const
223 {
224 if (!isLeft(point, m_topLeft, m_bottomLeft))
225 return false;
226 if (!isLeft(point, m_bottomLeft, m_bottomRight))
227 return false;
228 if (!isLeft(point, m_bottomRight, m_topRight))
229 return false;
230 if (!isLeft(point, m_topRight, m_topLeft))
231 return false;
232 return true;
233 }
234
isLeft(const vpImagePoint & pointToTest,const vpImagePoint & point1,const vpImagePoint & point2) const235 bool vpRectOriented::isLeft(const vpImagePoint &pointToTest, const vpImagePoint &point1,
236 const vpImagePoint &point2) const
237 {
238 double a = point1.get_j() - point2.get_j();
239 double b = point2.get_i() - point1.get_i();
240 double c = -(a * point1.get_i() + b * point1.get_j());
241 double d = a * pointToTest.get_i() + b * pointToTest.get_j() + c;
242 return (d > 0);
243 }
244