[Personalrobots-commit] SF.net SVN: personalrobots: [106] pkg/trunk/libTF/simple

[<tf...@us...>]

Revision: 106
          http://personalrobots.svn.sourceforge.net/personalrobots/?rev=106&view=rev
Author:   tfoote
Date:     2008-04-16 15:16:17 -0700 (Wed, 16 Apr 2008)

Log Message:
-----------
first cut at quaternion class

Modified Paths:
--------------
    pkg/trunk/libTF/simple/Makefile

Added Paths:
-----------
    pkg/trunk/libTF/simple/Quaternion3D.cc
    pkg/trunk/libTF/simple/Quaternion3D.hh
    pkg/trunk/libTF/simple/testQuat.cc

Modified: pkg/trunk/libTF/simple/Makefile
===================================================================
--- pkg/trunk/libTF/simple/Makefile	2008-04-16 21:16:16 UTC (rev 105)
+++ pkg/trunk/libTF/simple/Makefile	2008-04-16 22:16:17 UTC (rev 106)
@@ -3,8 +3,13 @@
 
 all: test
 
+#%.o:%.cc
 
+#%.o:%.hh
 
+quat: testQuat.cc Quaternion3D.o
+	g++ -g -O2 $^ -o quat -lnewmat
+
 test: main.cc libTF.o
 	g++ -g -O2 $^ -o test -lnewmat
 

Added: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc	                        (rev 0)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc	2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,73 @@
+#include "Quaternion3D.hh"
+
+Euler3D::Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll) :
+  x(_x),y(_y),z(_z),yaw(_yaw),pitch(_pitch),roll(_roll)
+{
+  return;
+};
+
+
+Quaternion3D::Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w):
+  xt(_xt),yt(_yt),zt(_zt),xr(_xr),yr(_yr),zr(_zr),w(_w)
+{
+  Normalize();
+  return;
+};
+
+
+void Quaternion3D::Normalize()
+{
+  double mag = getMagnitude();
+  xr /= mag;
+  yr /= mag;
+  zr /= mag;
+  w /= mag;
+};
+
+double Quaternion3D::getMagnitude()
+{
+  return sqrt(xr*xr + yr*yr + zr*zr + w*w);
+};
+
+
+NEWMAT::Matrix Quaternion3D::asMatrix()
+{
+  NEWMAT::Matrix outMat(4,4);
+  
+  double * mat = outMat.Store();
+
+  // math from http://www.j3d.org/matrix_faq/matrfaq_latest.html
+  double xx      = xr * xr;
+  double xy      = xr * yr;
+  double xz      = xr * zr;
+  double xw      = xr * w;
+  double yy      = yr * yr;
+  double yz      = yr * zr;
+  double yw      = yr * w;
+  double zz      = zr * zr;
+  double zw      = zr * w;
+  mat[0]  = 1 - 2 * ( yy + zz );
+  mat[4]  =     2 * ( xy - zw );
+  mat[8]  =     2 * ( xz + yw );
+  mat[1]  =     2 * ( xy + zw );
+  mat[5]  = 1 - 2 * ( xx + zz );
+  mat[9]  =     2 * ( yz - xw );
+  mat[2]  =     2 * ( xz - yw );
+  mat[6]  =     2 * ( yz + xw );
+  mat[10] = 1 - 2 * ( xx + yy );
+  mat[12]  = mat[13] = mat[14] = 0;
+  mat[3] = xt;
+  mat[7] = yt;
+  mat[11] = zt;
+  mat[15] = 1;
+    
+
+  return outMat;
+};
+
+
+void Quaternion3D::printMatrix()
+{
+  std::cout << asMatrix();
+
+};

Added: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh	                        (rev 0)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh	2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,54 @@
+#ifndef QUATERNION3D_HH
+#define QUATERNION3D_HH
+
+#include <iostream>
+#include <newmat/newmat.h>
+#include <newmat/newmatio.h>
+#include <math.h>
+
+class Euler3D {
+public:
+  //Constructor
+  Euler3D(double _x, double _y, double _z, double _yaw, double _pitch, double _roll);
+
+  //Storage
+  double  x,y,z,yaw,pitch,roll;
+
+  
+
+};
+
+class Quaternion3D {
+public:
+  //Constructor
+  Quaternion3D(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w);
+
+  inline void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w)
+  {xt = _xt; yt = _yt; zt = _zt; xr = _xr; yr = _yr; zr = _zr; w = _w;} ;
+
+  void Normalize();
+  double getMagnitude();
+
+  NEWMAT::Matrix asMatrix();
+  void printMatrix();
+
+
+private:
+  //Quaternion Storage
+  double xt,yt,zt,xr,yr,zr,w;
+
+
+};
+
+
+
+
+
+
+
+
+
+
+
+
+#endif //QUATERNION3D_HH

Added: pkg/trunk/libTF/simple/testQuat.cc
===================================================================
--- pkg/trunk/libTF/simple/testQuat.cc	                        (rev 0)
+++ pkg/trunk/libTF/simple/testQuat.cc	2008-04-16 22:16:17 UTC (rev 106)
@@ -0,0 +1,13 @@
+
+
+#include "Quaternion3D.hh"
+
+
+int main()
+{
+  Quaternion3D myquat(1,1,1,1,1,1,1);
+
+  myquat.printMatrix();
+
+  return 0;
+}


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.