[Jderobot-admin] jderobot-r934 - trunk/src/libs/geometry/math

ahcorde en jderobot.org ahcorde en jderobot.org
Lun Jun 17 10:17:14 CEST 2013


Author: ahcorde
Date: 2013-06-17 10:16:14 +0200 (Mon, 17 Jun 2013)
New Revision: 934

Added:
   trunk/src/libs/geometry/math/plano.cpp
   trunk/src/libs/geometry/math/plano.h
   trunk/src/libs/geometry/math/recta.cpp
   trunk/src/libs/geometry/math/recta.h
   trunk/src/libs/geometry/math/segmento.cpp
   trunk/src/libs/geometry/math/segmento.h
Log:
[ahcorde]


Added: trunk/src/libs/geometry/math/plano.cpp
===================================================================
--- trunk/src/libs/geometry/math/plano.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/plano.cpp	2013-06-17 08:16:14 UTC (rev 934)
@@ -0,0 +1,142 @@
+#include "plano.h"
+
+Plano::Plano(float A, float B, float C, float D)
+{
+    this->A = A;
+    this->B = B;
+    this->C = C;
+    this->D = D;
+}
+
+//http://paulbourke.net/geometry/pointlineplane/
+Plano::Plano(math::Vector3 p1, math::Vector3 p2, math::Vector3 p3)
+{
+    this->A = p1.getY()*(p2.getZ() - p3.getZ()) + p2.getY()*(p3.getZ() - p1.getZ()) + p3.getY()*(p1.getZ() - p2.getZ());
+
+    this->B = p1.getZ()*(p2.getX() - p3.getX()) + p2.getZ()*(p3.getX() - p1.getX()) + p3.getZ()*(p1.getX() - p2.getX());
+
+    this->C = p1.getX()*(p2.getY() - p3.getY()) + p2.getX()*(p3.getY() - p1.getY()) + p3.getX()*(p1.getY() - p2.getY());
+
+    this->D = -(p1.getX()*(p2.getY()*p3.getZ() - p3.getY()*p2.getZ()) + p2.getX()*(p3.getY()*p1.getZ() - p1.getY()*p3.getZ()) + p3.getX()*(p1.getY()*p2.getZ() - p2.getY()*p1.getZ()));
+
+}
+
+
+float Plano::getCoefA()
+{
+    return A;
+}
+
+float Plano::getCoefB()
+{
+    return B;
+}
+
+float Plano::getCoefC()
+{
+    return C;
+}
+
+float Plano::getCoefD()
+{
+    return D;
+}
+
+void Plano::setCoefA(float f)
+{
+    this->A = f;
+}
+
+void Plano::setCoefB(float f)
+{
+    this->B = f;
+}
+
+void Plano::setCoefC(float f)
+{
+    this->C = f;
+}
+
+void Plano::setCoefD(float f)
+{
+    this->D = f;
+}
+
+float Plano::distanciaAPunto(float x, float y, float z)
+{
+    float numerador = A*x + B*y + C*z + D;
+    float denominador = sqrt( pow(A, 2) + pow(B, 2) + pow(C, 2));
+
+    if(denominador!=0)
+        return numerador/denominador;
+    return 10000000;
+}
+
+float Plano::distanciaAPunto(math::Vector3 p)
+{
+    float numerador = A*p.getX() + B*p.getY() + C*p.getZ() + D;
+    float denominador = sqrt( pow(A, 2) + pow(B, 2) + pow(C, 2));
+
+    if(denominador!=0)
+        return numerador/denominador;
+    return 10000000;
+}
+
+float Plano::calculateU(float A, float B, float C, float D, float px, float py, float pz, float qx, float qy, float qz){
+    if( (A* (px-qx) + B* (py - qy) + C *( pz - qz ) != 0 ) )
+       return ( A * px + B * py + C * pz + D ) / (A* (px-qx) + B* (py - qy) + C *( pz - qz ) ) ;
+    return -1;
+}
+
+math::Vector3 Plano::InterConRecta(math::Vector3 p, math::Vector3 q)
+{
+    math::Vector3 result(0,0,0);
+
+    float u = calculateU(A, B, C, D, p.getX(), p.getY(), p.getZ(), q.getX(), q.getY(), q.getZ()) ;
+    //std::cout << "U: " << u << std::endl;
+    if( u <= 1 && u >= 0){
+        result.setX(p.getX() + u*(q.getX()-p.getX()));
+        result.setY(p.getY() + u*(q.getY()-p.getY()));
+        result.setZ(p.getZ() + u*(q.getZ()-p.getZ()));
+        //std::cout << "X: " << X  << " Y: " << Y << " Z: " << Z << std::endl;
+    }
+    return result;
+}
+
+math::Vector3 Plano::InterConRecta(float px, float py, float pz, float qx, float qy, float qz)
+{
+    math::Vector3 result(0,0,0);
+
+    float u = calculateU(A, B, C, D, px, py, pz, qx, qy, qz) ;
+    //std::cout << "U: " << u << std::endl;
+    if( u <= 1 && u >= 0){
+        result.setX(px + u*(qx-px));
+        result.setY(py + u*(qy-py));
+        result.setZ(pz + u*(qz-pz));
+        //std::cout << "X: " << X  << " Y: " << Y << " Z: " << Z << std::endl;
+    }
+    return result;
+}
+
+//parametricas
+// x = px + vx*t
+// y = py + vy*t
+// z = pz + vz*t
+
+// A* (px + vx*t) + B(py + vy*t) +C(pz+vz*t) + D = 0;
+// (-A*px - D - B*py - C*pz) = (A *vx *t + B *vy *t + C *vz *t)
+// t = (-A*px - D - B*py - C*pz)/ (A*vx + B*vy + C* vz)
+math::Vector3 Plano::proyeccionOrtogonal(float px, float py, float pz, float vx, float vy, float vz)
+{
+    float t = (-A*px - B*py - C*pz - D )/(A*vx + B*vy + C*vz );
+    math::Vector3 result(px+vx*t, py+vy*t, pz+vz*t);
+    return result;
+}
+
+math::Vector3 Plano::proyeccionOrtogonal(math::Vector3 p, float vx, float vy, float vz)
+{
+    float t = (-A*p.getX() - B*p.getY() - C*p.getZ() - D )/(A*vx + B*vy + C*vz );
+    math::Vector3 result(p.getX()+vx*t, p.getY()+vy*t, p.getZ()+vz*t);
+    return result;
+}
+

Added: trunk/src/libs/geometry/math/plano.h
===================================================================
--- trunk/src/libs/geometry/math/plano.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/plano.h	2013-06-17 08:16:14 UTC (rev 934)
@@ -0,0 +1,41 @@
+#ifndef PLANO_H
+#define PLANO_H
+
+#include <math.h>
+
+#include "vector3.h"
+
+class Plano
+{
+public:
+    Plano(float A, float B, float C, float D);
+    Plano(math::Vector3 p1, math::Vector3 p2, math::Vector3 p3);
+
+    float getCoefA();
+    float getCoefB();
+    float getCoefC();
+    float getCoefD();
+
+    void setCoefA(float f);
+    void setCoefB(float f);
+    void setCoefC(float f);
+    void setCoefD(float f);
+
+    math::Vector3 InterConRecta(math::Vector3 p, math::Vector3 q);
+    math::Vector3 InterConRecta(float px, float py, float pz, float qx, float qy, float qz);
+    float calculateU(float A, float B, float C, float D, float px, float py, float pz, float qx, float qy, float qz);
+    float distanciaAPunto(float x, float y, float z);
+    float distanciaAPunto(math::Vector3 p);
+
+    math::Vector3 proyeccionOrtogonal(float px, float py, float pz, float vx, float vy, float vz);
+    math::Vector3 proyeccionOrtogonal(math::Vector3 p, float vx, float vy, float vz);
+
+private:
+    float A;
+    float B;
+    float C;
+    float D;
+
+};
+
+#endif // PLANO_H

Added: trunk/src/libs/geometry/math/recta.cpp
===================================================================
--- trunk/src/libs/geometry/math/recta.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/recta.cpp	2013-06-17 08:16:14 UTC (rev 934)
@@ -0,0 +1,73 @@
+#include "recta.h"
+
+Recta::Recta()
+{
+	this->m = 0;
+	this->c = 0;
+}
+
+Recta::Recta(float m, float c)
+{
+	this->m = m;
+	this->c = c;
+}
+
+
+Recta::~Recta()
+{
+}
+
+Recta Recta::Perpendicular (float PuntoX, float PuntoY)
+{
+	Recta Recta_Perp;
+  
+    if ( fabs(this->m) < 0.001 ){
+        Recta_Perp.m = infinito /*( 1 / ( Recta.m * ( -1 ) ) );*/ ;
+
+		Recta_Perp.c =  PuntoX;  
+	}else{
+        Recta_Perp.m = -( 1 / ( this->m ) );
+
+        Recta_Perp.c = ( ( -Recta_Perp.m *  PuntoX  ) + PuntoY );
+	}
+	return Recta_Perp;
+  
+}
+
+Recta Recta::Paralela_Der_Dist (float  distancia, float x)
+{
+  Recta recta_salida;
+
+   if( fabs(this->m) < 1000 ){
+      recta_salida.m = this->m;
+      recta_salida.c = this->c;
+
+      recta_salida.c = recta_salida.c - distancia;
+
+   }else{
+       recta_salida.m = infinito;
+       recta_salida.c = x - distancia;
+   }
+  return recta_salida;
+
+}
+Recta Recta::Paralela_Izq_Dist ( float  distancia, float x)
+{
+  Recta recta_salida;
+
+  if( fabs(this->m) < 1000 ){
+      recta_salida.m = this->m;
+      recta_salida.c = this->c;
+
+      recta_salida.c = recta_salida.c + distancia;
+
+  }else{
+      recta_salida.m = infinito;
+      recta_salida.c = x + distancia;
+  }
+
+
+
+  return recta_salida;
+
+}


Property changes on: trunk/src/libs/geometry/math/recta.cpp
___________________________________________________________________
Added: svn:executable
   + *

Added: trunk/src/libs/geometry/math/recta.h
===================================================================
--- trunk/src/libs/geometry/math/recta.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/recta.h	2013-06-17 08:16:14 UTC (rev 934)
@@ -0,0 +1,24 @@
+#ifndef Recta_H
+#define Recta_H
+
+#include <math.h>	
+
+#define infinito 9.9e9
+
+class Recta{
+	public:
+		Recta();
+		Recta(float m, float c);
+		~Recta();
+		
+		Recta Perpendicular (float PuntoX, float PuntoY);
+		
+        Recta Paralela_Der_Dist (float distancia, float x);
+        Recta Paralela_Izq_Dist (float distancia, float x);
+		
+		float m;
+		float c;
+	private:
+};
+
+#endif


Property changes on: trunk/src/libs/geometry/math/recta.h
___________________________________________________________________
Added: svn:executable
   + *

Added: trunk/src/libs/geometry/math/segmento.cpp
===================================================================
--- trunk/src/libs/geometry/math/segmento.cpp	                        (rev 0)
+++ trunk/src/libs/geometry/math/segmento.cpp	2013-06-17 08:16:14 UTC (rev 934)
@@ -0,0 +1,49 @@
+#include "segmento.h"
+
+Segmento::Segmento()
+{
+}
+
+Segmento::Segmento(float x1, float y1, float z1, float x2, float y2, float z2)
+{
+	this->x1 = x1;
+	this->y1 = y1;
+	this->z1 = z1;
+	this->x2 = x2;
+	this->y2 = y2;
+	this->z2 = z2;
+}
+
+Segmento::Segmento(math::Vector3 p, math::Vector3 q)
+{
+    this->x1 = p.getX();
+    this->y1 = p.getY();
+    this->z1 = p.getZ();
+    this->x2 = q.getX();
+    this->y2 = q.getY();
+    this->z2 = q.getZ();
+}
+
+
+Segmento::~Segmento()
+{
+}
+
+Recta Segmento::SegmentoARecta( )
+{
+        Recta rx;
+
+    if( fabs( x2 - x1 ) < ( 0.00001 )){//abs// //son rectas paralelas, intersectan en el infinito//
+		rx.m = infinito;
+		rx.c = x1;	
+	}else{
+		rx.m = (y2 - y1) / (x2 - x1); // (m = (y2- y1)/ (x2-x1)) Pendiente
+		rx.c = y1 - ( rx.m* x1 );
+	}
+	
+	return rx;
+}
+
+
+
+


Property changes on: trunk/src/libs/geometry/math/segmento.cpp
___________________________________________________________________
Added: svn:executable
   + *

Added: trunk/src/libs/geometry/math/segmento.h
===================================================================
--- trunk/src/libs/geometry/math/segmento.h	                        (rev 0)
+++ trunk/src/libs/geometry/math/segmento.h	2013-06-17 08:16:14 UTC (rev 934)
@@ -0,0 +1,30 @@
+#ifndef Segmento_H
+#define Segmento_H
+
+#include <math.h>	
+
+#include "recta.h"
+#include "vector3.h"
+
+#define infinito 9.9e9
+
+class Segmento{
+	public:
+		Segmento();
+        Segmento(float x1, float y1, float z1, float x2, float y2, float z2);
+        Segmento(math::Vector3 p, math::Vector3 q);
+        ~Segmento();
+		
+		Recta SegmentoARecta();
+
+		float x1;
+		float y1;
+		float z1;
+		
+		float x2;
+		float y2;
+		float z2;
+	private:
+};
+
+#endif


Property changes on: trunk/src/libs/geometry/math/segmento.h
___________________________________________________________________
Added: svn:executable
   + *



More information about the Jderobot-admin mailing list