[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