[Jderobot-admin] jderobot-r964 - in trunk/src/libs/geometry: . math
eperdices en jderobot.org
eperdices en jderobot.org
Lun Ago 5 21:18:23 CEST 2013
Author: eperdices
Date: 2013-08-05 21:17:22 +0200 (Mon, 05 Aug 2013)
New Revision: 964
Added:
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
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
trunk/src/libs/geometry/math/Segment2D.cpp
trunk/src/libs/geometry/math/Segment2D.h
trunk/src/libs/geometry/math/geoconst.h
Log:
A?\195?\177adidos planos y rectas 3D
Modified: trunk/src/libs/geometry/CMakeLists.txt
===================================================================
--- trunk/src/libs/geometry/CMakeLists.txt 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/CMakeLists.txt 2013-08-05 19:17:22 UTC (rev 964)
@@ -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/Segment2D.cpp math/Point2D.cpp math/Point3D.cpp collada/colladaparser.cpp collada/color.cpp collada/malla.cpp collada/material.cpp collada/submalla.cpp progeo/Progeo.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 math/Line3D.cpp math/Plane3D collada/colladaparser.cpp collada/color.cpp collada/malla.cpp collada/material.cpp collada/submalla.cpp progeo/Progeo.cpp math/segmento.cpp math/recta.cpp)
ADD_LIBRARY(geometry SHARED ${SRC_FILES})
Modified: trunk/src/libs/geometry/math/Line2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Line2D.cpp 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Line2D.cpp 2013-08-05 19:17:22 UTC (rev 964)
@@ -100,6 +100,56 @@
return this->getNormalLine(p.getPoint()(0), p.getPoint()(1));
}
+double
+Line2D::distanceTo(Point2D &p) {
+ /*dist = |A*x + B*y + C|
+ ---------------
+ sqrt(A^2 + B^2)*/
+
+ if(this->v(0) == 0.0 && this->v(1) == 0.0)
+ return G_INFINITE;
+
+ return abs(this->v(0)*p.getPoint()(0) + this->v(1)*p.getPoint()(1) + this->v(2))/(sqrt(G_SQUARE(this->v(0)) + G_SQUARE(this->v(1))));
+}
+
+double
+Line2D::distanceToOrigin() {
+ /*dist = |C|
+ ---------------
+ sqrt(A^2 + B^2)*/
+
+ if(this->v(0) == 0.0 && this->v(1) == 0.0)
+ return G_INFINITE;
+
+ return abs(this->v(2))/(sqrt(G_SQUARE(this->v(0)) + G_SQUARE(this->v(1))));
+}
+
+double
+Line2D::getAngle() {
+ double alpha;
+
+ if(this->v(1) == 0.0)
+ return G_PI_2;
+
+ alpha = atan(-this->v(0)/this->v(1));
+
+ /*Normalize*/
+ if(alpha < 0)
+ alpha += G_PI;
+ if(alpha > G_PI)
+ alpha -= G_PI;
+
+ return alpha;
+}
+
+double
+Line2D::getGradient() {
+ if(this->v(1) == 0.0)
+ return G_INFINITE;
+
+ return -this->v(0)/this->v(1);
+}
+
Point2D
Line2D::intersectLine(Line2D &l) {
double x,y,h;
@@ -122,7 +172,21 @@
return this->v(0) * p.getPoint()(0) + this->v(1) * p.getPoint()(1) + this->v(2) == 0;
}
+bool
+Line2D::parallelTo(Line2D &l, double threshold) {
+ double diff;
+ diff = this->getAngle() - l.getAngle();
+
+ /*Normalize*/
+ while(diff < -G_PI_2)
+ diff += G_PI;
+ while(diff > G_PI_2)
+ diff -= G_PI;
+
+ return fabs(diff) < threshold;
+}
+
/*
Recta Recta::Perpendicular (float PuntoX, float PuntoY)
{
Modified: trunk/src/libs/geometry/math/Line2D.h
===================================================================
--- trunk/src/libs/geometry/math/Line2D.h 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Line2D.h 2013-08-05 19:17:22 UTC (rev 964)
@@ -30,6 +30,7 @@
#include <math.h>
#include <eigen3/Eigen/Dense>
+#include "geoconst.h"
class Point2D;
@@ -53,11 +54,24 @@
Line2D getNormalLine(double px, double py);
Line2D getNormalLine(Point2D &p);
- /*Intersect two lines into a 2D Point*/
+ /*Distance between line and point*/
+ double distanceTo(Point2D &p);
+
+ /*Distance between line and the coordinates origin (0,0)*/
+ double distanceToOrigin();
+
+ /*Calc positive angle*/
+ double getAngle();
+ double getGradient();
+
+ /*Intersect two lines into a 2D Point. If intersection is not valid Point2D is set at the infinite*/
Point2D intersectLine(Line2D &l);
/*Return true if the 2D line has a concrete 2D Point*/
bool hasPoint(Point2D &p);
+
+ /*Compare parallel lines with a threshold*/
+ bool parallelTo(Line2D &l, double threshold);
private:
Added: trunk/src/libs/geometry/math/Line3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Line3D.cpp (rev 0)
+++ trunk/src/libs/geometry/math/Line3D.cpp 2013-08-05 19:17:22 UTC (rev 964)
@@ -0,0 +1,110 @@
+/*
+ *
+ * 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>
+ *
+ */
+
+#include "Line3D.h"
+#include "Point3D.h"
+#include "Plane3D.h"
+
+Line3D::Line3D() {
+ this->v.resize(6);
+ this->m.resize(4,4);
+ this->v.setZero();
+}
+
+Line3D::Line3D(Point3D &p1, Point3D &p2) {
+ this->v.resize(6);
+ this->m.resize(4,4);
+ this->v = this->getLine(p1, p2);
+}
+
+Line3D::Line3D(Plane3D &p1, Plane3D &p2) {
+ this->v.resize(6);
+ this->m.resize(4,4);
+ this->v = this->getLine(p1, p2);
+}
+
+Eigen::VectorXd&
+Line3D::getVector() {
+ return this->v;
+}
+
+Eigen::VectorXd
+Line3D::getLine(Point3D &p1, Point3D &p2) {
+ Eigen::VectorXd v(6);
+
+ this->m = p1.getPoint() * p2.getPoint().transpose() - p2.getPoint() * p1.getPoint().transpose();
+ this->plucker_matrix2vector(this->m, v);
+ return v;
+}
+
+Eigen::VectorXd
+Line3D::getLine(Plane3D &p1, Plane3D &p2) {
+ Eigen::VectorXd v(6);
+
+ this->m = p1.getPlane() * p2.getPlane().transpose() - p2.getPlane() * p1.getPlane().transpose();
+ this->plucker_matrix2vector(this->m, v);
+ this->plucker_vector_swap(v);
+ return v;
+}
+
+Plane3D
+Line3D::toPlane(Point3D &p) {
+ Eigen::Vector4d plane;
+
+ this->plucker_vector2matrix(this->m, this->v);
+ plane = this->m * p.getPoint();
+ return Plane3D(plane);
+}
+
+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);
+}
+
+void
+Line3D::plucker_vector2matrix(Eigen::MatrixXd &m, Eigen::VectorXd &v) {
+ Eigen::MatrixXd mtmp;
+
+ m = Eigen::MatrixXd::Zero(4, 4);
+ m(0,1) = v(0);
+ m(0,2) = v(1);
+ m(0,3) = v(2);
+ m(1,2) = v(3);
+ m(3,1) = v(4);
+ m(2,3) = v(5);
+ mtmp = m.transpose();
+ m = m-mtmp;
+}
+
+void
+Line3D::plucker_vector_swap(Eigen::VectorXd &v) {
+ Eigen::VectorXd vaux(6);
+
+ vaux = v;
+ v(0) = vaux(5);
+ v(1) = vaux(4);
+ v(2) = vaux(3);
+ v(3) = vaux(2);
+ v(4) = vaux(1);
+ v(5) = vaux(0);
+}
Added: trunk/src/libs/geometry/math/Line3D.h
===================================================================
--- trunk/src/libs/geometry/math/Line3D.h (rev 0)
+++ trunk/src/libs/geometry/math/Line3D.h 2013-08-05 19:17:22 UTC (rev 964)
@@ -0,0 +1,64 @@
+/*
+ *
+ * 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>
+ *
+ *
+ *
+ * Line3D: Represents a 3D line in Plücker coordinates
+ */
+
+#ifndef LINE3D_H
+#define LINE3D_H
+#define EIGEN_DONT_ALIGN_STATICALLY True
+
+#include <math.h>
+#include <eigen3/Eigen/Dense>
+#include "geoconst.h"
+
+class Point3D;
+class Plane3D;
+
+class Line3D {
+public:
+ Line3D();
+ Line3D(Point3D &p1, Point3D &p2);
+ Line3D(Plane3D &p1, Plane3D &p2);
+
+ Eigen::VectorXd& getVector();
+
+ /*Calculate line from 2 3D points or 2 3D planes*/
+ Eigen::VectorXd getLine(Point3D &p1, Point3D &p2);
+ Eigen::VectorXd getLine(Plane3D &p1, Plane3D &p2);
+
+ /*Create a plane from a 3D point and current line*/
+ Plane3D toPlane(Point3D &p);
+
+private:
+
+ void plucker_matrix2vector(Eigen::MatrixXd &m, Eigen::VectorXd &v);
+ void plucker_vector2matrix(Eigen::MatrixXd &m, Eigen::VectorXd &v);
+ void plucker_vector_swap(Eigen::VectorXd &v);
+
+ Eigen::VectorXd v;
+ Eigen::MatrixXd m;
+
+};
+
+#endif
Added: trunk/src/libs/geometry/math/Plane3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Plane3D.cpp (rev 0)
+++ trunk/src/libs/geometry/math/Plane3D.cpp 2013-08-05 19:17:22 UTC (rev 964)
@@ -0,0 +1,75 @@
+/*
+ *
+ * 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>
+ *
+ */
+
+#include "Plane3D.h"
+#include "Point3D.h"
+#include "Line3D.h"
+
+Plane3D::Plane3D() {
+ this->plane.setZero();
+}
+
+Plane3D::Plane3D(Point3D &p1, Point3D &p2, Point3D &p3) {
+ this->plane = this->getPlane(p1, p2, p3);
+}
+
+Plane3D::Plane3D(Line3D &l, Point3D &p) {
+ this->plane = l.toPlane(p).plane;
+}
+
+Plane3D::Plane3D(Eigen::Vector4d p) {
+ this->plane = p;
+}
+
+Eigen::Vector4d&
+Plane3D::getPlane() {
+ return this->plane;
+}
+
+Eigen::Vector4d
+Plane3D::getPlane(Point3D &p1, Point3D &p2, Point3D &p3) {
+ Eigen::Vector4d p;
+
+ p(0) = p1.getPoint()(1)*(p2.getPoint()(2) - p3.getPoint()(2)) + p2.getPoint()(1)*(p3.getPoint()(2) - p1.getPoint()(2)) + p3.getPoint()(1)*(p1.getPoint()(2) - p2.getPoint()(2));
+
+ p(1) = p1.getPoint()(2)*(p2.getPoint()(0) - p3.getPoint()(0)) + p2.getPoint()(2)*(p3.getPoint()(0) - p1.getPoint()(0)) + p3.getPoint()(2)*(p1.getPoint()(0) - p2.getPoint()(0));
+
+ p(2) = p1.getPoint()(0)*(p2.getPoint()(1) - p3.getPoint()(1)) + p2.getPoint()(0)*(p3.getPoint()(1) - p1.getPoint()(1)) + p3.getPoint()(0)*(p1.getPoint()(1) - p2.getPoint()(1));
+
+ p(3) = -(p1.getPoint()(0)*(p2.getPoint()(1)*p3.getPoint()(2) - p3.getPoint()(1)*p2.getPoint()(2)) + p2.getPoint()(0)*(p3.getPoint()(1)*p1.getPoint()(2) - p1.getPoint()(1)*p3.getPoint()(2)) + p3.getPoint()(0)*(p1.getPoint()(1)*p2.getPoint()(2) - p2.getPoint()(1)*p1.getPoint()(2)));
+
+ return p;
+}
+
+Line3D
+Plane3D::intersectPlane(Plane3D &p) {
+ return Line3D(*this, p);
+}
+
+Plane3D &
+Plane3D::operator =(const Plane3D &p) {
+ this->plane = p.plane;
+
+ return *this;
+}
+
Added: trunk/src/libs/geometry/math/Plane3D.h
===================================================================
--- trunk/src/libs/geometry/math/Plane3D.h (rev 0)
+++ trunk/src/libs/geometry/math/Plane3D.h 2013-08-05 19:17:22 UTC (rev 964)
@@ -0,0 +1,62 @@
+/*
+ *
+ * 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>
+ *
+ *
+ *
+ * Plane3D: Represents a 3D plane with the general equation of the plane: Ax+By+Cz+D = 0
+ */
+
+#ifndef PLANE3D_H
+#define PLANE3D_H
+#define EIGEN_DONT_ALIGN_STATICALLY True
+
+#include <math.h>
+#include <eigen3/Eigen/Dense>
+#include "geoconst.h"
+
+class Point3D;
+class Line3D;
+
+class Plane3D {
+public:
+ Plane3D();
+ Plane3D(Point3D &p1, Point3D &p2, Point3D &p3);
+ Plane3D(Line3D &l, Point3D &p);
+ Plane3D(Eigen::Vector4d p);
+
+ Eigen::Vector4d& getPlane();
+
+ /*Calculate plane from 3 3D points*/
+ Eigen::Vector4d getPlane(Point3D &p1, Point3D &p2, Point3D &p3);
+
+ /*Intersect two planes into a 3D line*/
+ Line3D intersectPlane(Plane3D &p);
+
+ /*Operators*/
+ Plane3D &operator =(const Plane3D &p);
+
+private:
+
+ Eigen::Vector4d plane;
+
+};
+
+#endif
Modified: trunk/src/libs/geometry/math/Point2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point2D.cpp 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Point2D.cpp 2013-08-05 19:17:22 UTC (rev 964)
@@ -46,11 +46,26 @@
return this->point;
}
+bool
+Point2D::isInfinite() {
+ return this->point(2) == 0.0;
+}
+
double
Point2D::distanceTo(Point2D &p) {
return sqrt(G_SQUARE(this->point(0)-p.point(0)) + G_SQUARE(this->point(1)-p.point(1)));
}
+double
+Point2D::distanceTo(Line2D &l) {
+ return l.distanceTo(*this);
+}
+
+double
+Point2D::distanceTo(Segment2D &s) {
+ return s.distanceTo(*this);
+}
+
bool
Point2D::isInsideSegment(Segment2D &s) {
double tmp;
@@ -100,7 +115,7 @@
Point2D &
Point2D::operator =(const Point2D &pt) {
- this->point = getPoint();
+ this->point = pt.point;
return *this;
}
Modified: trunk/src/libs/geometry/math/Point2D.h
===================================================================
--- trunk/src/libs/geometry/math/Point2D.h 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Point2D.h 2013-08-05 19:17:22 UTC (rev 964)
@@ -44,8 +44,13 @@
Eigen::Vector3d& getPoint();
- /*Distance between 2D points*/
+ /*Return true if the point is at the infinite*/
+ bool isInfinite();
+
+ /*Distance between 2D objects*/
double distanceTo(Point2D &p);
+ double distanceTo(Line2D &l);
+ double distanceTo(Segment2D &s);
/*Check if the point is inside a 2D segment*/
bool isInsideSegment(Segment2D &s);
Modified: trunk/src/libs/geometry/math/Point3D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Point3D.cpp 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Point3D.cpp 2013-08-05 19:17:22 UTC (rev 964)
@@ -44,6 +44,11 @@
return this->point;
}
+bool
+Point3D::isInfinite() {
+ return this->point(3) == 0.0;
+}
+
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)));
Modified: trunk/src/libs/geometry/math/Point3D.h
===================================================================
--- trunk/src/libs/geometry/math/Point3D.h 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Point3D.h 2013-08-05 19:17:22 UTC (rev 964)
@@ -41,6 +41,9 @@
Eigen::Vector4d& getPoint();
+ /*Return true if the point is at the infinite*/
+ bool isInfinite();
+
/*Distance between 3D points*/
double distanceTo(Point3D &p);
Modified: trunk/src/libs/geometry/math/Segment2D.cpp
===================================================================
--- trunk/src/libs/geometry/math/Segment2D.cpp 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Segment2D.cpp 2013-08-05 19:17:22 UTC (rev 964)
@@ -57,6 +57,57 @@
return Line2D(*(this->pstart),*(this->pend));
}
+double
+Segment2D::distanceTo(Point2D &p) {
+ double pos;
+
+ /*Position in segment*/
+ pos = p.getPositionInSegment(*this);
+
+ if(pos >= 0 && pos <= 1)
+ return this->toLine().distanceTo(p);
+
+ if(pos < 0)
+ return pstart->distanceTo(p);
+ else
+ return pend->distanceTo(p);
+}
+
+double
+Segment2D::getAngle() {
+ double alpha;
+ double diffx, diffy;
+
+ diffx = this->pend->getPoint()(0) - this->pstart->getPoint()(0);
+ diffy = this->pend->getPoint()(1) - this->pstart->getPoint()(1);
+
+ if(diffy == 0.0)
+ return G_PI_2;
+
+ alpha = atan(-diffx/diffy);
+
+ /*Normalize*/
+ if(alpha < 0)
+ alpha += G_PI;
+ if(alpha > G_PI)
+ alpha -= G_PI;
+
+ return alpha;
+}
+
+double
+Segment2D::getGradient() {
+ double diffx, diffy;
+
+ diffx = this->pend->getPoint()(0) - this->pstart->getPoint()(0);
+ diffy = this->pend->getPoint()(1) - this->pstart->getPoint()(1);
+
+ if(diffy == 0.0)
+ return G_INFINITE;
+
+ return -diffx/diffy;
+}
+
Point2D
Segment2D::getPointInPosition(double u) {
double px, py;
@@ -72,3 +123,39 @@
Segment2D::hasPoint(Point2D &p) {
return this->toLine().hasPoint(p) && p.isInsideSegment(*this);
}
+
+Point2D
+Segment2D::intersectSegment(Segment2D &s) {
+ Point2D p2d;
+ Line2D l1, l2;
+
+ /*Calc line intersection*/
+ l1 = this->toLine();
+ l2 = s.toLine();
+ p2d = l1.intersectLine(l2);
+
+ /*Check if p2d is valid*/
+ if(p2d.isInfinite())
+ return Point2D(0.0,0.0,0.0);
+
+ /*Check extremes*/
+ if(p2d.isInsideSegment(*this) && p2d.isInsideSegment(s))
+ return p2d;
+
+ return Point2D(0.0,0.0,0.0);
+}
+
+bool
+Segment2D::parallelTo(Segment2D &s, double threshold) {
+ double diff;
+
+ diff = this->getAngle() - s.getAngle();
+
+ /*Normalize*/
+ while(diff < -G_PI_2)
+ diff += G_PI;
+ while(diff > G_PI_2)
+ diff -= G_PI;
+
+ return fabs(diff) < threshold;
+}
Modified: trunk/src/libs/geometry/math/Segment2D.h
===================================================================
--- trunk/src/libs/geometry/math/Segment2D.h 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/Segment2D.h 2013-08-05 19:17:22 UTC (rev 964)
@@ -52,11 +52,24 @@
/*Convert 2D segment into a 2D line*/
Line2D toLine();
+ /*Distance between segment and point*/
+ double distanceTo(Point2D &p);
+
+ /*Calc positive angle*/
+ double getAngle();
+ double getGradient();
+
/*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);
+
+ /*Intersect two segments into a 2D Point. If intersection is not valid Point2D is set at the infinite*/
+ Point2D intersectSegment(Segment2D &s);
+
+ /*Compare parallel segments with a threshold*/
+ bool parallelTo(Segment2D &s, double threshold);
private:
Modified: trunk/src/libs/geometry/math/geoconst.h
===================================================================
--- trunk/src/libs/geometry/math/geoconst.h 2013-08-05 05:00:50 UTC (rev 963)
+++ trunk/src/libs/geometry/math/geoconst.h 2013-08-05 19:17:22 UTC (rev 964)
@@ -6,6 +6,10 @@
#define G_PI 3.1415926535897932384626433832795
#endif
+#ifndef G_PI_2
+#define G_PI_2 1.570796327
+#endif
+
#ifndef G_INFINITE
#define G_INFINITE 9.9e9
#endif
More information about the Jderobot-admin
mailing list