[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