[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