1 | /*
|
---|
2 | * Plane.cpp
|
---|
3 | *
|
---|
4 | * Created on: Apr 7, 2010
|
---|
5 | * Author: crueger
|
---|
6 | */
|
---|
7 |
|
---|
8 | #include "Plane.hpp"
|
---|
9 | #include "vector.hpp"
|
---|
10 | #include "defs.hpp"
|
---|
11 | #include "info.hpp"
|
---|
12 | #include "log.hpp"
|
---|
13 | #include "verbose.hpp"
|
---|
14 | #include "Helpers/Assert.hpp"
|
---|
15 | #include <cmath>
|
---|
16 |
|
---|
17 | /**
|
---|
18 | * generates a plane from three given vectors defining three points in space
|
---|
19 | */
|
---|
20 | Plane::Plane(const Vector &y1, const Vector &y2, const Vector &y3) throw(LinearDependenceException) :
|
---|
21 | normalVector(new Vector())
|
---|
22 | {
|
---|
23 | Vector x1 = y1 -y2;
|
---|
24 | Vector x2 = y3 -y2;
|
---|
25 | if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON) || (fabs(x1.Angle(x2)) < MYEPSILON)) {
|
---|
26 | throw LinearDependenceException(__FILE__,__LINE__);
|
---|
27 | }
|
---|
28 | // Log() << Verbose(4) << "relative, first plane coordinates:";
|
---|
29 | // x1.Output((ofstream *)&cout);
|
---|
30 | // Log() << Verbose(0) << endl;
|
---|
31 | // Log() << Verbose(4) << "second plane coordinates:";
|
---|
32 | // x2.Output((ofstream *)&cout);
|
---|
33 | // Log() << Verbose(0) << endl;
|
---|
34 |
|
---|
35 | normalVector->at(0) = (x1[1]*x2[2] - x1[2]*x2[1]);
|
---|
36 | normalVector->at(1) = (x1[2]*x2[0] - x1[0]*x2[2]);
|
---|
37 | normalVector->at(2) = (x1[0]*x2[1] - x1[1]*x2[0]);
|
---|
38 | normalVector->Normalize();
|
---|
39 |
|
---|
40 | offset=normalVector->ScalarProduct(y1);
|
---|
41 | }
|
---|
42 | /**
|
---|
43 | * Constructs a plane from two direction vectors and a offset.
|
---|
44 | * If no offset is given a plane through origin is assumed
|
---|
45 | */
|
---|
46 | Plane::Plane(const Vector &y1, const Vector &y2, double _offset) throw(ZeroVectorException,LinearDependenceException) :
|
---|
47 | normalVector(new Vector()),
|
---|
48 | offset(_offset)
|
---|
49 | {
|
---|
50 | Vector x1 = y1;
|
---|
51 | Vector x2 = y2;
|
---|
52 | if ((fabs(x1.Norm()) < MYEPSILON) || (fabs(x2.Norm()) < MYEPSILON)) {
|
---|
53 | throw ZeroVectorException(__FILE__,__LINE__);
|
---|
54 | }
|
---|
55 |
|
---|
56 | if((fabs(x1.Angle(x2)) < MYEPSILON)) {
|
---|
57 | throw LinearDependenceException(__FILE__,__LINE__);
|
---|
58 | }
|
---|
59 | // Log() << Verbose(4) << "relative, first plane coordinates:";
|
---|
60 | // x1.Output((ofstream *)&cout);
|
---|
61 | // Log() << Verbose(0) << endl;
|
---|
62 | // Log() << Verbose(4) << "second plane coordinates:";
|
---|
63 | // x2.Output((ofstream *)&cout);
|
---|
64 | // Log() << Verbose(0) << endl;
|
---|
65 |
|
---|
66 | normalVector->at(0) = (x1[1]*x2[2] - x1[2]*x2[1]);
|
---|
67 | normalVector->at(1) = (x1[2]*x2[0] - x1[0]*x2[2]);
|
---|
68 | normalVector->at(2) = (x1[0]*x2[1] - x1[1]*x2[0]);
|
---|
69 | normalVector->Normalize();
|
---|
70 | }
|
---|
71 |
|
---|
72 | Plane::Plane(const Vector &_normalVector, double _offset) throw(ZeroVectorException):
|
---|
73 | normalVector(new Vector(_normalVector)),
|
---|
74 | offset(_offset)
|
---|
75 | {
|
---|
76 | if(normalVector->IsZero())
|
---|
77 | throw ZeroVectorException(__FILE__,__LINE__);
|
---|
78 | double factor = 1/normalVector->Norm();
|
---|
79 | // normalize the plane parameters
|
---|
80 | (*normalVector)*=factor;
|
---|
81 | offset*=factor;
|
---|
82 | }
|
---|
83 |
|
---|
84 | Plane::Plane(const Vector &_normalVector, const Vector &_offsetVector) throw(ZeroVectorException):
|
---|
85 | normalVector(new Vector(_normalVector))
|
---|
86 | {
|
---|
87 | if(normalVector->IsZero()){
|
---|
88 | throw ZeroVectorException(__FILE__,__LINE__);
|
---|
89 | }
|
---|
90 | normalVector->Normalize();
|
---|
91 | offset = normalVector->ScalarProduct(_offsetVector);
|
---|
92 | }
|
---|
93 |
|
---|
94 | /**
|
---|
95 | * copy constructor
|
---|
96 | */
|
---|
97 | Plane::Plane(const Plane& plane) :
|
---|
98 | normalVector(new Vector(*plane.normalVector)),
|
---|
99 | offset(plane.offset)
|
---|
100 | {}
|
---|
101 |
|
---|
102 |
|
---|
103 | Plane::~Plane()
|
---|
104 | {}
|
---|
105 |
|
---|
106 |
|
---|
107 | Vector Plane::getNormal() const{
|
---|
108 | return *normalVector;
|
---|
109 | }
|
---|
110 |
|
---|
111 | double Plane::getOffset() const{
|
---|
112 | return offset;
|
---|
113 | }
|
---|
114 |
|
---|
115 | Vector Plane::getOffsetVector() {
|
---|
116 | return getOffset()*getNormal();
|
---|
117 | }
|
---|
118 |
|
---|
119 | vector<Vector> Plane::getPointsOnPlane(){
|
---|
120 | std::vector<Vector> res;
|
---|
121 | res.reserve(3);
|
---|
122 | // first point on the plane
|
---|
123 | res.push_back(getOffsetVector());
|
---|
124 | // get a vector that has direction of plane
|
---|
125 | Vector direction;
|
---|
126 | direction.GetOneNormalVector(getNormal());
|
---|
127 | res.push_back(res[0]+direction);
|
---|
128 | // get an orthogonal vector to direction and normal (has direction of plane)
|
---|
129 | direction.VectorProduct(getNormal());
|
---|
130 | direction.Normalize();
|
---|
131 | res.push_back(res[0] +direction);
|
---|
132 | return res;
|
---|
133 | }
|
---|
134 |
|
---|
135 |
|
---|
136 | /** Calculates the intersection point between a line defined by \a *LineVector and \a *LineVector2 and a plane defined by \a *Normal and \a *PlaneOffset.
|
---|
137 | * According to [Bronstein] the vectorial plane equation is:
|
---|
138 | * -# \f$\stackrel{r}{\rightarrow} \cdot \stackrel{N}{\rightarrow} + D = 0\f$,
|
---|
139 | * where \f$\stackrel{r}{\rightarrow}\f$ is the vector to be testet, \f$\stackrel{N}{\rightarrow}\f$ is the plane's normal vector and
|
---|
140 | * \f$D = - \stackrel{a}{\rightarrow} \stackrel{N}{\rightarrow}\f$, the offset with respect to origin, if \f$\stackrel{a}{\rightarrow}\f$,
|
---|
141 | * is an offset vector onto the plane. The line is parametrized by \f$\stackrel{x}{\rightarrow} + k \stackrel{t}{\rightarrow}\f$, where
|
---|
142 | * \f$\stackrel{x}{\rightarrow}\f$ is the offset and \f$\stackrel{t}{\rightarrow}\f$ the directional vector (NOTE: No need to normalize
|
---|
143 | * the latter). Inserting the parametrized form into the plane equation and solving for \f$k\f$, which we insert then into the parametrization
|
---|
144 | * of the line yields the intersection point on the plane.
|
---|
145 | * \param *Origin first vector of line
|
---|
146 | * \param *LineVector second vector of line
|
---|
147 | * \return true - \a this contains intersection point on return, false - line is parallel to plane (even if in-plane)
|
---|
148 | */
|
---|
149 | Vector Plane::GetIntersection(const Vector &Origin, const Vector &LineVector)
|
---|
150 | {
|
---|
151 | Info FunctionInfo(__func__);
|
---|
152 | Vector res;
|
---|
153 |
|
---|
154 | // find intersection of a line defined by Offset and Direction with a plane defined by triangle
|
---|
155 | Vector Direction = LineVector - Origin;
|
---|
156 | Direction.Normalize();
|
---|
157 | Log() << Verbose(1) << "INFO: Direction is " << Direction << "." << endl;
|
---|
158 | //Log() << Verbose(1) << "INFO: PlaneNormal is " << *PlaneNormal << " and PlaneOffset is " << *PlaneOffset << "." << endl;
|
---|
159 | double factor1 = Direction.ScalarProduct(*normalVector.get());
|
---|
160 | if (fabs(factor1) < MYEPSILON) { // Uniqueness: line parallel to plane?
|
---|
161 | Log() << Verbose(1) << "BAD: Line is parallel to plane, no intersection." << endl;
|
---|
162 | throw LinearDependenceException(__FILE__,__LINE__);
|
---|
163 | }
|
---|
164 |
|
---|
165 | double factor2 = Origin.ScalarProduct(*normalVector.get());
|
---|
166 | if (fabs(factor2-offset) < MYEPSILON) { // Origin is in-plane
|
---|
167 | Log() << Verbose(1) << "GOOD: Origin of line is in-plane." << endl;
|
---|
168 | res = Origin;
|
---|
169 | return res;
|
---|
170 | }
|
---|
171 |
|
---|
172 | double scaleFactor = (offset-factor2)/factor1;
|
---|
173 |
|
---|
174 | //factor = Origin->ScalarProduct(PlaneNormal)*(-PlaneOffset->ScalarProduct(PlaneNormal))/(Direction.ScalarProduct(PlaneNormal));
|
---|
175 | Direction.Scale(scaleFactor);
|
---|
176 | res = Origin + Direction;
|
---|
177 | Log() << Verbose(1) << "INFO: Scaled direction is " << Direction << "." << endl;
|
---|
178 |
|
---|
179 | // test whether resulting vector really is on plane
|
---|
180 | ASSERT(fabs(res.ScalarProduct(*normalVector) - offset) < MYEPSILON,
|
---|
181 | "Calculated line-Plane intersection does not lie on plane.");
|
---|
182 | return res;
|
---|
183 | };
|
---|
184 |
|
---|
185 | /************ Methods inherited from Space ****************/
|
---|
186 |
|
---|
187 | double Plane::distance(const Vector &point) const{
|
---|
188 | double res = point.ScalarProduct(*normalVector)-offset;
|
---|
189 | return fabs(res);
|
---|
190 | }
|
---|
191 |
|
---|
192 | Vector Plane::getClosestPoint(const Vector &point) const{
|
---|
193 | double factor = point.ScalarProduct(*normalVector)-offset;
|
---|
194 | if(fabs(factor) < MYEPSILON){
|
---|
195 | // the point itself lies on the plane
|
---|
196 | return point;
|
---|
197 | }
|
---|
198 | Vector difference = factor * (*normalVector);
|
---|
199 | return (point - difference);
|
---|
200 | }
|
---|
201 |
|
---|
202 | // Operators
|
---|
203 |
|
---|
204 | ostream &operator << (ostream &ost,const Plane &p){
|
---|
205 | ost << "<" << p.getNormal() << ";x> - " << p.getOffset() << "=0";
|
---|
206 | return ost;
|
---|
207 | }
|
---|