source: rrlib_geometry/tPlane.hpp @ 60:a22756932ab5

Last change on this file since 60:a22756932ab5 was 60:a22756932ab5, checked in by Tobias Föhst <foehst@…>, 6 years ago

Added and updated license information

File size: 7.9 KB
Line 
1//
2// You received this file as part of RRLib
3// Robotics Research Library
4//
5// Copyright (C) Finroc GbR (finroc.org)
6//
7// This program is free software; you can redistribute it and/or modify
8// it under the terms of the GNU General Public License as published by
9// the Free Software Foundation; either version 2 of the License, or
10// (at your option) any later version.
11//
12// This program is distributed in the hope that it will be useful,
13// but WITHOUT ANY WARRANTY; without even the implied warranty of
14// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
15// GNU General Public License for more details.
16//
17// You should have received a copy of the GNU General Public License along
18// with this program; if not, write to the Free Software Foundation, Inc.,
19// 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA.
20//
21//----------------------------------------------------------------------
22/*!\file    tPlane.hpp
23 *
24 * \author  Tobias Foehst
25 *
26 * \date    2011-01-19
27 *
28 */
29//----------------------------------------------------------------------
30
31//----------------------------------------------------------------------
32// External includes (system with <>, local with "")
33//----------------------------------------------------------------------
34#include "rrlib/math/utilities.h"
35
36//----------------------------------------------------------------------
37// Internal includes with ""
38//----------------------------------------------------------------------
39
40//----------------------------------------------------------------------
41// Debugging
42//----------------------------------------------------------------------
43#include <cassert>
44
45//----------------------------------------------------------------------
46// Namespace declaration
47//----------------------------------------------------------------------
48namespace rrlib
49{
50namespace geometry
51{
52
53//----------------------------------------------------------------------
54// Forward declarations / typedefs / enums
55//----------------------------------------------------------------------
56
57//----------------------------------------------------------------------
58// Const values
59//----------------------------------------------------------------------
60
61//----------------------------------------------------------------------
62// Implementation
63//----------------------------------------------------------------------
64
65//----------------------------------------------------------------------
66// tPlane constructors
67//----------------------------------------------------------------------
68template <size_t Tdimension, typename TElement>
69tPlane<Tdimension, TElement>::tPlane()
70{
71  this->normal[0] = 1;
72}
73
74template <size_t Tdimension, typename TElement>
75tPlane<Tdimension, TElement>::tPlane(const typename tShape::tPoint &support, const math::tVector<Tdimension, TElement> &normal)
76  : support(support),
77    normal(normal.Normalized())
78{}
79
80template <size_t Tdimension, typename TElement>
81tPlane<Tdimension, TElement>::tPlane(const typename tShape::tPoint &p1, const typename tShape::tPoint &p2, const typename tShape::tPoint &p3)
82  : support(p1),
83    normal(CrossProduct(p2 - p1, p3 - p1).Normalized())
84{}
85
86//----------------------------------------------------------------------
87// tPlane Set
88//----------------------------------------------------------------------
89template <size_t Tdimension, typename TElement>
90void tPlane<Tdimension, TElement>::Set(const typename tShape::tPoint &support, const math::tVector<Tdimension, TElement> &normal)
91{
92  this->support = support;
93  this->normal = normal.Normalized();
94  this->SetChanged();
95}
96
97template <size_t Tdimension, typename TElement>
98void tPlane<Tdimension, TElement>::Set(const typename tShape::tPoint &p1, const typename tShape::tPoint &p2, const typename tShape::tPoint &p3)
99{
100  this->support = p1;
101  this->normal = CrossProduct(p2 - p1, p3 - p1).Normalized();
102  this->SetChanged();
103}
104
105//----------------------------------------------------------------------
106// tPlane Evaluation: operator ()
107//----------------------------------------------------------------------
108//template <size_t Tdimension, typename TElement>
109//const typename tShape<Tdimension, TElement>::tPoint tPlane<Tdimension, TElement>::operator()(tParameter t) const
110//{
111//  return math::tVector<Tdimension, TElement>(this->support + t * this->direction);
112//}
113
114//----------------------------------------------------------------------
115// tPlane GetDistanceToPoint
116//----------------------------------------------------------------------
117template <size_t Tdimension, typename TElement>
118const TElement tPlane<Tdimension, TElement>::GetDistanceToPoint(const typename tShape::tPoint &point) const
119{
120  return math::AbsoluteValue(this->normal * (point - this->support));
121}
122
123//----------------------------------------------------------------------
124// tPlane GetClosestPoint
125//----------------------------------------------------------------------
126template <size_t Tdimension, typename TElement>
127const typename tShape<Tdimension, TElement>::tPoint tPlane<Tdimension, TElement>::GetClosestPoint(const typename tShape::tPoint &reference_point) const
128{
129  return -(this->normal * (reference_point - this->support)) * this->normal + reference_point;
130}
131
132//----------------------------------------------------------------------
133// tPlane Translate
134//----------------------------------------------------------------------
135template <size_t Tdimension, typename TElement>
136tPlane<Tdimension, TElement> &tPlane<Tdimension, TElement>::Translate(const math::tVector<Tdimension, TElement> &translation)
137{
138  this->support += translation;
139  this->SetChanged();
140  return *this;
141}
142
143//----------------------------------------------------------------------
144// tPlane Rotate
145//----------------------------------------------------------------------
146template <size_t Tdimension, typename TElement>
147tPlane<Tdimension, TElement> &tPlane<Tdimension, TElement>::Rotate(const math::tMatrix<Tdimension, Tdimension, TElement> &rotation)
148{
149  assert(math::IsEqual(rotation.Determinant(), 1));
150  this->support = rotation * this->support;
151  this->normal = rotation * this->normal;
152  this->SetChanged();
153  return *this;
154}
155
156//----------------------------------------------------------------------
157// tPlane Transform
158//----------------------------------------------------------------------
159template <size_t Tdimension, typename TElement>
160tPlane<Tdimension, TElement> &tPlane<Tdimension, TElement>::Transform(const math::tMatrix < Tdimension + 1, Tdimension + 1, TElement > &transformation)
161{
162#ifndef NDEBUG
163  for (size_t i = 0; i < Tdimension; ++i)
164  {
165    assert(math::IsEqual(transformation[Tdimension][i], 0));
166  }
167  assert(math::IsEqual(transformation[Tdimension][Tdimension], 1));
168#endif
169  math::tMatrix<Tdimension, Tdimension, TElement> rotation;
170  for (size_t row = 0; row < Tdimension; ++row)
171  {
172    for (size_t column = 0; column < Tdimension; ++column)
173    {
174      rotation[row][column] = transformation[row][column];
175    }
176  }
177  assert(math::IsEqual(rotation.Determinant(), 1));
178
179  this->support = transformation.MultiplyHomogeneously(this->support);
180  this->normal = rotation * this->normal;
181  this->SetChanged();
182  return *this;
183}
184
185//----------------------------------------------------------------------
186// tPlane UpdateBoundingBox
187//----------------------------------------------------------------------
188template <size_t Tdimension, typename TElement>
189void tPlane<Tdimension, TElement>::UpdateBoundingBox(typename tShape::tBoundingBox &bounding_box) const
190{}
191
192//----------------------------------------------------------------------
193// tPlane UpdateCenterOfGravity
194//----------------------------------------------------------------------
195template <size_t Tdimension, typename TElement>
196void tPlane<Tdimension, TElement>::UpdateCenterOfGravity(typename tShape::tPoint &center_of_gravity) const
197{
198  center_of_gravity = this->GetClosestPoint(tShape::tPoint::Zero());
199}
200
201//----------------------------------------------------------------------
202// End of namespace declaration
203//----------------------------------------------------------------------
204}
205}
Note: See TracBrowser for help on using the repository browser.