[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