[Jderobot-admin] jderobot-r961 - in trunk/src/libs/geometry: . math

eperdices en jderobot.org eperdices en jderobot.org
Vie Ago 2 19:48:01 CEST 2013


Author: eperdices
Date: 2013-08-02 19:47:01 +0200 (Fri, 02 Aug 2013)
New Revision: 961

Added:
   trunk/src/libs/geometry/math/Segment2D.cpp
   trunk/src/libs/geometry/math/Segment2D.h
Modified:
   trunk/src/libs/geometry/CMakeLists.txt
   trunk/src/libs/geometry/math/Line2D.cpp
   trunk/src/libs/geometry/math/Line2D.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
Log:
A?\195?\177adidas unas cuantas funciones ?\195?\186tiles entre puntos, segmentos y l?\195?\173neas


Modified: trunk/src/libs/geometry/CMakeLists.txt
===================================================================
--- trunk/src/libs/geometry/CMakeLists.txt	2013-08-02 12:53:30 UTC (rev 960)
+++ trunk/src/libs/geometry/CMakeLists.txt	2013-08-02 17:47:01 UTC (rev 961)
@@ -4,7 +4,7 @@
  
 INCLUDE_DIRECTORIES(${CMAKE_CURRENT_SOURCE_DIR} /usr/include/libxml2/)
 
-set(SRC_FILES math/matriz3x3.cpp  math/matriz4x4.cpp  math/plano.cpp  math/utils.cpp  math/vector2d.cpp  math/vector3.cpp  math/vector2H.cpp math/vector3H.cpp math/Line2D.cpp math/Point2D.cpp math/Point3D.cpp collada/colladaparser.cpp collada/color.cpp collada/malla.cpp collada/material.cpp collada/submalla.cpp progeo/progeo2.cpp math/segmento.cpp math/recta.cpp) 
+set(SRC_FILES math/matriz3x3.cpp  math/matriz4x4.cpp  math/plano.cpp  math/utils.cpp  math/vector2d.cpp  math/vector3.cpp  math/vector2H.cpp math/vector3H.cpp math/Line2D.cpp math/Segment2D.cpp  math/Point2D.cpp math/Point3D.cpp collada/colladaparser.cpp collada/color.cpp collada/malla.cpp collada/material.cpp collada/submalla.cpp progeo/progeo2.cpp math/segmento.cpp math/recta.cpp) 
  
 ADD_LIBRARY(geometry_shared SHARED ${SRC_FILES})
 

Modified: trunk/src/libs/geometry/math/Line2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Line2D.cpp	2013-08-02 12:53:30 UTC (rev 960)
+++ trunk/src/libs/geometry/math/Line2D.cpp	2013-08-02 17:47:01 UTC (rev 961)
@@ -22,6 +22,7 @@
  */
 
 #include "Line2D.h"
+#include "Point2D.h"
 
 Line2D::Line2D() {
   this->v.setZero();
@@ -31,11 +32,11 @@
   this->v = this->getLine(p1x, p1y, p2x, p2y);
 }
 
-Line2D::Line2D(Eigen::Vector2d p1, Eigen::Vector2d p2) {
+Line2D::Line2D(Eigen::Vector2d &p1, Eigen::Vector2d &p2) {
   this->v = this->getLine(p1, p2);
 }
 
-Line2D::Line2D(Point2D p1, Point2D p2) {
+Line2D::Line2D(Point2D &p1, Point2D &p2) {
   this->v = this->getLine(p1, p2);
 }
 
@@ -43,11 +44,11 @@
   this->v << va, vb, vc;
 }
 
-Line2D::Line2D(Eigen::Vector3d v) {
+Line2D::Line2D(Eigen::Vector3d &v) {
   this->v = v;
 }
 
-Eigen::Vector3d
+Eigen::Vector3d&
 Line2D::getVector() {
   return this->v;
 }
@@ -65,7 +66,7 @@
 }
 
 Eigen::Vector3d
-Line2D::getLine(Eigen::Vector2d p1, Eigen::Vector2d p2) {
+Line2D::getLine(Eigen::Vector2d &p1, Eigen::Vector2d &p2) {
   Eigen::Vector3d v;
 
   /*Get the Ax + By + C = 0 parameters*/
@@ -77,7 +78,7 @@
 }
 
 Eigen::Vector3d
-Line2D::getLine(Point2D p1, Point2D p2) {
+Line2D::getLine(Point2D &p1, Point2D &p2) {
   return this->getLine(p1.getPoint()(0), p1.getPoint()(1), p2.getPoint()(0), p2.getPoint()(1));
 }
 
@@ -95,14 +96,33 @@
 }
 
 Line2D
-Line2D::getNormalLine(Point2D p) {
+Line2D::getNormalLine(Point2D &p) {
   return this->getNormalLine(p.getPoint()(0), p.getPoint()(1));
 }
 
+Point2D
+Line2D::intersectLine(Line2D &l) {
+	double x,y,h;
+  Point2D p;
 
+	h = this->v(0)*l.v(1) - this->v(1)*l.v(0); 		  /*x1*y2 - y1*x2*/
 
+	/*Are parallel*/	
+	if(h==0)
+		return Point2D(0.0,0.0,0.0);
+  else {
+	  x = (this->v(1)*l.v(2) - l.v(1)*this->v(2))/h; /*y1*z2 - z1*y2*/
+	  y = (this->v(2)*l.v(0) - this->v(0)*l.v(2))/h; /*z1*x2 - x1*z2*/
+    return Point2D(x,y,1.0);
+  }
+}
 
+bool
+Line2D::hasPoint(Point2D &p) {
+  return this->v(0) * p.getPoint()(0) + this->v(1) * p.getPoint()(1) + this->v(2) == 0;
+}
 
+
 /*
 Recta Recta::Perpendicular (float PuntoX, float PuntoY)
 {

Modified: trunk/src/libs/geometry/math/Line2D.h
===================================================================
--- trunk/src/libs/geometry/math/Line2D.h	2013-08-02 12:53:30 UTC (rev 960)
+++ trunk/src/libs/geometry/math/Line2D.h	2013-08-02 17:47:01 UTC (rev 961)
@@ -30,27 +30,34 @@
 
 #include <math.h>
 #include <eigen3/Eigen/Dense>
-#include "Point2D.h"
 
+class Point2D;
+
 class Line2D {
 public:
   Line2D();
   Line2D(double p1x, double p1y, double p2x, double p2y);
-  Line2D(Eigen::Vector2d p1, Eigen::Vector2d p2);
-  Line2D(Point2D p1, Point2D p2);
+  Line2D(Eigen::Vector2d &p1, Eigen::Vector2d &p2);
+  Line2D(Point2D &p1, Point2D &p2);
   Line2D(double va, double vb, double vc);
-  Line2D(Eigen::Vector3d v);
+  Line2D(Eigen::Vector3d &v);
 
-  Eigen::Vector3d getVector();
+  Eigen::Vector3d& getVector();
 
   /*Calculate line from 2 2D points*/
   Eigen::Vector3d getLine(double p1x, double p1y, double p2x, double p2y);
-  Eigen::Vector3d getLine(Eigen::Vector2d p1, Eigen::Vector2d p2);
-  Eigen::Vector3d getLine(Point2D p1, Point2D p2);
+  Eigen::Vector3d getLine(Eigen::Vector2d &p1, Eigen::Vector2d &p2);
+  Eigen::Vector3d getLine(Point2D &p1, Point2D &p2);
 
   /*Calculate a 2D normal line from current 2D line and a 2D point*/
   Line2D getNormalLine(double px, double py);
-  Line2D getNormalLine(Point2D p);
+  Line2D getNormalLine(Point2D &p);
+
+  /*Intersect two lines into a 2D Point*/
+  Point2D intersectLine(Line2D &l);
+
+  /*Return true if the 2D line has a concrete 2D Point*/
+  bool hasPoint(Point2D &p);
   
 private:
 

Modified: trunk/src/libs/geometry/math/Point2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point2D.cpp	2013-08-02 12:53:30 UTC (rev 960)
+++ trunk/src/libs/geometry/math/Point2D.cpp	2013-08-02 17:47:01 UTC (rev 961)
@@ -22,6 +22,8 @@
  */
 
 #include "Point2D.h"
+#include "Segment2D.h"
+#include "Line2D.h"
 
 Point2D::Point2D() {
   this->point.setZero();
@@ -31,23 +33,75 @@
   this->point << x, y, h;
 }
 
-Point2D::Point2D(Eigen::Vector2d p, double h) {
+Point2D::Point2D(Eigen::Vector2d &p, double h) {
   this->point << p(0), p(1), h;
 }
 
-Point2D::Point2D(Eigen::Vector3d p) {
+Point2D::Point2D(Eigen::Vector3d &p) {
   this->point = p;
 }
 
-Eigen::Vector3d
+Eigen::Vector3d&
 Point2D::getPoint() {
   return this->point;
 }
 
 double
-Point2D::distanceTo(Point2D p) {
+Point2D::distanceTo(Point2D &p) {
   return sqrt(G_SQUARE(this->point(0)-p.point(0)) + G_SQUARE(this->point(1)-p.point(1)));
 }
 
+bool
+Point2D::isInsideSegment(Segment2D &s) {
+  double tmp;
 
+  if(s.isPoint())
+    return false;
 
+  tmp = this->getPositionInSegment(s);
+
+  if(tmp >=0 && tmp <=1)
+    return true;
+
+  return false;
+}
+
+double
+Point2D::getPositionInSegment(Segment2D &s) {
+  double tmp;
+  Point2D ps, pe;
+
+  /*  (Px-Ax)(Bx-Ax) + (Py-Ay)(By-Ay)
+  u = -------------------------------
+      (Bx-Ax)^2 + (By-Ay)^2*/
+  
+  /*Check if the line is a point*/
+  if(s.isPoint())
+    return 0;
+
+  ps = s.getPointStart();
+  pe = s.getPointEnd();
+
+  tmp = (this->point(0)-ps.point(0))*(pe.point(0)-ps.point(0)) + (this->point(1)-ps.point(1))*(pe.point(1)-ps.point(1));
+  tmp = tmp /(G_SQUARE(pe.point(0)-ps.point(0)) + G_SQUARE(pe.point(1)-ps.point(1)));
+
+  return tmp;
+}
+
+bool
+Point2D::belongsToLine(Line2D &l) {
+  return l.hasPoint(*this);
+}
+
+bool
+Point2D::belongsToSegment(Segment2D &s) {
+  return s.hasPoint(*this);
+}
+
+Point2D &
+Point2D::operator =(const Point2D &pt) {
+  this->point = getPoint();
+
+  return *this;
+}
+

Modified: trunk/src/libs/geometry/math/Point2D.h
===================================================================
--- trunk/src/libs/geometry/math/Point2D.h	2013-08-02 12:53:30 UTC (rev 960)
+++ trunk/src/libs/geometry/math/Point2D.h	2013-08-02 17:47:01 UTC (rev 961)
@@ -32,16 +32,35 @@
 #include <eigen3/Eigen/Dense>
 #include "geoconst.h"
 
+class Segment2D;
+class Line2D;
+
 class Point2D {
 public:
   Point2D();
   Point2D(double x, double y, double h=1.0);
-  Point2D(Eigen::Vector2d p, double h=1.0);
-  Point2D(Eigen::Vector3d p);
+  Point2D(Eigen::Vector2d &p, double h=1.0);
+  Point2D(Eigen::Vector3d &p);
 
-  Eigen::Vector3d getPoint();
+  Eigen::Vector3d& getPoint();
 
-  double distanceTo(Point2D p);
+  /*Distance between 2D points*/
+  double distanceTo(Point2D &p);
+
+  /*Check if the point is inside a 2D segment*/
+  bool isInsideSegment(Segment2D &s);
+
+	/*Get the position of the point according to a segment: Solve u in P = A+u(B-A)*/
+  double getPositionInSegment(Segment2D &s);
+
+  /*Return true if the point belongs to a 2D line*/
+  bool belongsToLine(Line2D &l);
+
+  /*Return true if the point belongs to a 2D segment*/
+  bool belongsToSegment(Segment2D &s);
+
+  /*Operators*/
+  Point2D &operator =(const Point2D &pt);
   
 private:
 

Modified: trunk/src/libs/geometry/math/Point3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point3D.cpp	2013-08-02 12:53:30 UTC (rev 960)
+++ trunk/src/libs/geometry/math/Point3D.cpp	2013-08-02 17:47:01 UTC (rev 961)
@@ -31,21 +31,21 @@
   this->point << x, y, z, h;
 }
 
-Point3D::Point3D(Eigen::Vector3d p, double h) {
+Point3D::Point3D(Eigen::Vector3d &p, double h) {
   this->point << p(0), p(1), p(2), h;
 }
 
-Point3D::Point3D(Eigen::Vector4d p) {
+Point3D::Point3D(Eigen::Vector4d &p) {
   this->point = p;
 }
 
-Eigen::Vector4d
+Eigen::Vector4d&
 Point3D::getPoint() {
   return this->point;
 }
 
 double
-Point3D::distanceTo(Point3D p) {
+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)));
 }
 

Modified: trunk/src/libs/geometry/math/Point3D.h
===================================================================
--- trunk/src/libs/geometry/math/Point3D.h	2013-08-02 12:53:30 UTC (rev 960)
+++ trunk/src/libs/geometry/math/Point3D.h	2013-08-02 17:47:01 UTC (rev 961)
@@ -36,12 +36,13 @@
 public:
   Point3D();
   Point3D(double x, double y, double z, double h=1.0);
-  Point3D(Eigen::Vector3d p, double h=1.0);
-  Point3D(Eigen::Vector4d p);
+  Point3D(Eigen::Vector3d &p, double h=1.0);
+  Point3D(Eigen::Vector4d &p);
 
-  Eigen::Vector4d getPoint();
+  Eigen::Vector4d& getPoint();
 
-  double distanceTo(Point3D p);
+  /*Distance between 3D points*/
+  double distanceTo(Point3D &p);
   
 private:
 

Added: trunk/src/libs/geometry/math/Segment2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Segment2D.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/Segment2D.cpp	2013-08-02 17:47:01 UTC (rev 961)
@@ -0,0 +1,74 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published this->pend->getPoint()(1)
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  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>
+ *
+ */
+
+#include "Segment2D.h"
+#include "Point2D.h"
+
+Segment2D::Segment2D() {
+}
+
+Segment2D::Segment2D(Point2D &p1, Point2D &p2) {
+  this->pstart = new Point2D(p1.getPoint());
+  this->pend = new Point2D(p2.getPoint());
+}
+
+Point2D&
+Segment2D::getPointStart() {
+  return *(this->pstart);
+}
+
+Point2D&
+Segment2D::getPointEnd() {
+  return *(this->pend);
+}
+
+double
+Segment2D::getLength() {
+  return this->pstart->distanceTo(*(this->pend));
+}
+
+bool
+Segment2D::isPoint() {
+  return this->pstart->getPoint() == this->pend->getPoint();
+}
+
+Line2D
+Segment2D::toLine() {
+  return Line2D(*(this->pstart),*(this->pend));
+}
+
+Point2D
+Segment2D::getPointInPosition(double u) {
+  double px, py;
+
+	/*Get P from the equation P = A+u(B-A)*/
+	px = this->pstart->getPoint()(0) + u*(this->pend->getPoint()(0)-this->pstart->getPoint()(0));
+	py = this->pstart->getPoint()(1) + u*(this->pend->getPoint()(1)-this->pstart->getPoint()(1));  
+
+  return Point2D(px, py, 1.0);
+}
+
+bool
+Segment2D::hasPoint(Point2D &p) {
+  return this->toLine().hasPoint(p) && p.isInsideSegment(*this);
+}

Added: trunk/src/libs/geometry/math/Segment2D.h
===================================================================
--- trunk/src/libs/geometry/math/Segment2D.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/Segment2D.h	2013-08-02 17:47:01 UTC (rev 961)
@@ -0,0 +1,68 @@
+/*
+ *
+ *  Copyright (C) 1997-2013 JDERobot Developers Team
+ *
+ *  This program is free software: you can redistribute it and/or modify
+ *  it under the terms of the GNU General Public License as published by
+ *  the Free Software Foundation, either version 3 of the License, or
+ *  (at your option) any later version.
+ *
+ *  This program is distributed in the hope that it will be useful,
+ *  but WITHOUT ANY WARRANTY; without even the implied warranty of
+ *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ *  GNU General Public License for more details.
+ *
+ *  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>
+ *
+ *
+ *
+ *  Segment2D: Represents a 2D Segment represented by 2 2D Points
+ */
+
+#ifndef SEGMENT2D_H
+#define SEGMENT2D_H
+#define EIGEN_DONT_ALIGN_STATICALLY True
+
+#include <math.h>
+#include <eigen3/Eigen/Dense>
+#include "geoconst.h"
+#include "Line2D.h"
+
+class Point2D;
+
+class Segment2D {
+public:
+  Segment2D();
+  Segment2D(Point2D &p1, Point2D &p2);
+
+  Point2D& getPointStart();
+  Point2D& getPointEnd();
+
+  /*Get segment length*/
+  double getLength();
+
+  /*Return true if the segment is a point*/
+  bool isPoint();
+
+  /*Convert 2D segment into a 2D line*/
+  Line2D toLine();
+
+	/*Return a 2D Point belonging to the segment: Solve P in P = A+u(B-A)*/
+  Point2D getPointInPosition(double u);
+
+  /*Return true if the 2D segment has a concrete 2D Point*/
+  bool hasPoint(Point2D &p);
+  
+private:
+
+  Point2D *pstart;
+  Point2D *pend;
+    
+};
+
+#endif



More information about the Jderobot-admin mailing list