[Jderobot-admin] jderobot-r966 - trunk/src/libs/geometry/math
eperdices en jderobot.org
eperdices en jderobot.org
Mar Ago 6 13:18:33 CEST 2013
Author: eperdices
Date: 2013-08-06 13:17:33 +0200 (Tue, 06 Aug 2013)
New Revision: 966
Modified:
trunk/src/libs/geometry/math/Line2D.cpp
trunk/src/libs/geometry/math/Line2D.h
trunk/src/libs/geometry/math/Line3D.cpp
trunk/src/libs/geometry/math/Line3D.h
trunk/src/libs/geometry/math/Plane3D.cpp
trunk/src/libs/geometry/math/Plane3D.h
trunk/src/libs/geometry/math/Point2D.cpp
trunk/src/libs/geometry/math/Point2D.h
trunk/src/libs/geometry/math/Point3D.cpp
trunk/src/libs/geometry/math/Point3D.h
trunk/src/libs/geometry/math/Segment2D.cpp
trunk/src/libs/geometry/math/Segment2D.h
trunk/src/libs/geometry/math/Segment3D.cpp
trunk/src/libs/geometry/math/Segment3D.h
trunk/src/libs/geometry/math/geoconst.h
Log:
Corregido bug en Line3D
Modified: trunk/src/libs/geometry/math/Line2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Line2D.cpp 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Line2D.cpp 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*/
@@ -187,6 +185,12 @@
return fabs(diff) < threshold;
}
+std::ostream&
+operator <<(std::ostream &o,const Line2D &l) {
+ o << l.v;
+ return o;
+}
+
/*
Recta Recta::Perpendicular (float PuntoX, float PuntoY)
{
Modified: trunk/src/libs/geometry/math/Line2D.h
===================================================================
--- trunk/src/libs/geometry/math/Line2D.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Line2D.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*
*
@@ -72,6 +70,9 @@
/*Compare parallel lines with a threshold*/
bool parallelTo(Line2D &l, double threshold);
+
+ /*Operators*/
+ friend std::ostream& operator <<(std::ostream &o,const Line2D &l);
private:
Modified: trunk/src/libs/geometry/math/Line3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Line3D.cpp 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Line3D.cpp 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*/
@@ -67,24 +65,46 @@
return v;
}
+
Plane3D
Line3D::toPlane(Point3D &p) {
+ Eigen::VectorXd v;
Eigen::Vector4d plane;
- this->plucker_vector2matrix(this->m, this->v);
+ v = this->v;
+ this->plucker_vector_swap(v);
+ this->plucker_vector2matrix(this->m, v);
plane = this->m * p.getPoint();
return Plane3D(plane);
}
Point3D
Line3D::intersectPlane(Plane3D &p) {
- Eigen::Vector4d point;
+ Eigen::Vector4d point;
this->plucker_vector2matrix(this->m, this->v);
point = this->m * p.getPlane();
return Point3D(point);
}
+bool
+Line3D::hasPoint(Point3D &p) {
+ Eigen::VectorXd v, res;
+
+ v = this->v;
+ this->plucker_vector_swap(v);
+ this->plucker_vector2matrix(this->m, v);
+ res = this->m*p.getPoint();
+
+ return res.isZero();
+}
+
+std::ostream&
+operator <<(std::ostream &o,const Line3D &l) {
+ o << l.v;
+ return o;
+}
+
void
Line3D::plucker_matrix2vector(Eigen::MatrixXd &m, Eigen::VectorXd &v) {
v << m(0,1), m(0,2), m(0,3), m(1,2), m(3,1), m(2,3);
Modified: trunk/src/libs/geometry/math/Line3D.h
===================================================================
--- trunk/src/libs/geometry/math/Line3D.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Line3D.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*
*
@@ -52,6 +50,12 @@
/*Intersect the line with a plane and get a 3D point*/
Point3D intersectPlane(Plane3D &p);
+
+ /*Return true if the 3D line has a concrete 3D Point*/
+ bool hasPoint(Point3D &p);
+
+ /*Operators*/
+ friend std::ostream& operator <<(std::ostream &o,const Line3D &l);
private:
Modified: trunk/src/libs/geometry/math/Plane3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Plane3D.cpp 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Plane3D.cpp 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*/
@@ -33,6 +31,7 @@
this->plane = this->getPlane(p1, p2, p3);
}
+
Plane3D::Plane3D(Line3D &l, Point3D &p) {
this->plane = l.toPlane(p).plane;
}
@@ -66,6 +65,11 @@
return Line3D(*this, p);
}
+bool
+Plane3D::hasPoint(Point3D &p) {
+ return this->plane(0) * p.getPoint()(0) + this->plane(1) * p.getPoint()(1) + this->plane(2) * p.getPoint()(2) + this->plane(3) == 0;
+}
+
Plane3D &
Plane3D::operator =(const Plane3D &p) {
this->plane = p.plane;
@@ -73,3 +77,9 @@
return *this;
}
+std::ostream&
+operator <<(std::ostream &o,const Plane3D &p) {
+ o << p.plane;
+ return o;
+}
+
Modified: trunk/src/libs/geometry/math/Plane3D.h
===================================================================
--- trunk/src/libs/geometry/math/Plane3D.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Plane3D.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*
*
@@ -50,8 +48,12 @@
/*Intersect two planes into a 3D line*/
Line3D intersectPlane(Plane3D &p);
+ /*Return true if the 3D plane has a concrete 3D Point*/
+ bool hasPoint(Point3D &p);
+
/*Operators*/
Plane3D &operator =(const Plane3D &p);
+ friend std::ostream& operator <<(std::ostream &o,const Plane3D &p);
private:
Modified: trunk/src/libs/geometry/math/Point2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point2D.cpp 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Point2D.cpp 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*/
@@ -51,6 +49,15 @@
return this->point(2) == 0.0;
}
+bool
+Point2D::normalize() {
+ if(this->point(2) == 0.0)
+ return false;
+
+ this->point = this->point/this->point(2);
+ return true;
+}
+
double
Point2D::distanceTo(Point2D &p) {
return sqrt(G_SQUARE(this->point(0)-p.point(0)) + G_SQUARE(this->point(1)-p.point(1)));
@@ -113,10 +120,15 @@
return s.hasPoint(*this);
}
-Point2D &
+Point2D&
Point2D::operator =(const Point2D &pt) {
this->point = pt.point;
return *this;
}
+std::ostream&
+operator <<(std::ostream &o,const Point2D &p) {
+ o << "(" << p.point(0) << "," << p.point(1) << "," << p.point(2) << ")";
+ return o;
+}
Modified: trunk/src/libs/geometry/math/Point2D.h
===================================================================
--- trunk/src/libs/geometry/math/Point2D.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Point2D.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*
*
@@ -47,6 +45,9 @@
/*Return true if the point is at the infinite*/
bool isInfinite();
+ /*Normaliza point. Return true the point is not the infinite*/
+ bool normalize();
+
/*Distance between 2D objects*/
double distanceTo(Point2D &p);
double distanceTo(Line2D &l);
@@ -65,8 +66,9 @@
bool belongsToSegment(Segment2D &s);
/*Operators*/
- Point2D &operator =(const Point2D &pt);
-
+ Point2D& operator =(const Point2D &pt);
+ friend std::ostream& operator <<(std::ostream &o,const Point2D &p);
+
private:
Eigen::Vector3d point;
Modified: trunk/src/libs/geometry/math/Point3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point3D.cpp 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Point3D.cpp 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,13 +15,13 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*/
#include "Point3D.h"
+#include "Line3D.h"
+#include "Plane3D.h"
Point3D::Point3D() {
this->point.setZero();
@@ -49,10 +49,33 @@
return this->point(3) == 0.0;
}
+bool
+Point3D::normalize() {
+ if(this->point(3) == 0.0)
+ return false;
+
+ this->point = this->point/this->point(3);
+ return true;
+}
+
double
Point3D::distanceTo(Point3D &p) {
return sqrt(G_SQUARE(this->point(0)-p.point(0)) + G_SQUARE(this->point(1)-p.point(1)) + + G_SQUARE(this->point(2)-p.point(2)));
}
+bool
+Point3D::belongsToLine(Line3D &l) {
+ return l.hasPoint(*this);
+}
+bool
+Point3D::belongsToPlane(Plane3D &p) {
+ return p.hasPoint(*this);
+}
+std::ostream&
+operator <<(std::ostream &o,const Point3D &p) {
+ o << "(" << p.point(0) << "," << p.point(1) << "," << p.point(2) << "," << p.point(3) << ")";
+ return o;
+}
+
Modified: trunk/src/libs/geometry/math/Point3D.h
===================================================================
--- trunk/src/libs/geometry/math/Point3D.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Point3D.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*
*
@@ -32,6 +30,9 @@
#include <eigen3/Eigen/Dense>
#include "geoconst.h"
+class Line3D;
+class Plane3D;
+
class Point3D {
public:
Point3D();
@@ -44,8 +45,20 @@
/*Return true if the point is at the infinite*/
bool isInfinite();
+ /*Normaliza point. Return true the point is not the infinite*/
+ bool normalize();
+
/*Distance between 3D points*/
double distanceTo(Point3D &p);
+
+ /*Return true if the point belongs to a 3D line*/
+ bool belongsToLine(Line3D &l);
+
+ /*Return true if the point belongs to a 3D plane*/
+ bool belongsToPlane(Plane3D &p);
+
+ /*Operators*/
+ friend std::ostream& operator <<(std::ostream &o,const Point3D &p);
private:
Modified: trunk/src/libs/geometry/math/Segment2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Segment2D.cpp 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Segment2D.cpp 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*/
@@ -161,3 +159,9 @@
return fabs(diff) < threshold;
}
+
+std::ostream&
+operator <<(std::ostream &o,const Segment2D &s) {
+ o << *s.pstart << " -> " << *s.pend;
+ return o;
+}
Modified: trunk/src/libs/geometry/math/Segment2D.h
===================================================================
--- trunk/src/libs/geometry/math/Segment2D.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Segment2D.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*
*
@@ -70,6 +68,9 @@
/*Compare parallel segments with a threshold*/
bool parallelTo(Segment2D &s, double threshold);
+
+ /*Operators*/
+ friend std::ostream& operator <<(std::ostream &o,const Segment2D &s);
private:
Modified: trunk/src/libs/geometry/math/Segment3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Segment3D.cpp 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Segment3D.cpp 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*/
@@ -59,3 +57,9 @@
return Line3D(*(this->pstart),*(this->pend));
}
+std::ostream&
+operator <<(std::ostream &o,const Segment3D &s) {
+ o << *s.pstart << " -> " << *s.pend;
+ return o;
+}
+
Modified: trunk/src/libs/geometry/math/Segment3D.h
===================================================================
--- trunk/src/libs/geometry/math/Segment3D.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/Segment3D.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -15,9 +15,7 @@
* You should have received a copy of the GNU General Public License
* along with this program. If not, see http://www.gnu.org/licenses/.
*
- * Authors : Alejandro Hernández <ahcorde [at] gmail [dot] com>
- * Roberto Calvo <rocapal [at] gsyc [dot] urjc [dot] es>
- * Eduardo Perdices <eperdices [at] gsyc [dot] es>
+ * Authors : Eduardo Perdices <eperdices [at] gsyc [dot] es>
*
*
*
@@ -51,6 +49,9 @@
/*Convert 3D segment into a 3D line*/
Line3D toLine();
+
+ /*Operators*/
+ friend std::ostream& operator <<(std::ostream &o,const Segment3D &s);
private:
Modified: trunk/src/libs/geometry/math/geoconst.h
===================================================================
--- trunk/src/libs/geometry/math/geoconst.h 2013-08-06 09:04:17 UTC (rev 965)
+++ trunk/src/libs/geometry/math/geoconst.h 2013-08-06 11:17:33 UTC (rev 966)
@@ -1,3 +1,5 @@
+#include <iostream>
+
#ifndef G_SQUARE
#define G_SQUARE(a) ( (a) * (a) )
#endif
More information about the Jderobot-admin
mailing list