Menu
▾
▴
[Personalrobots-commit] SF.net SVN: personalrobots: [121] pkg/trunk/libTF/simple
|
From: <tf...@us...> - 2008-04-17 23:37:39
|
Revision: 121
http://personalrobots.svn.sourceforge.net/personalrobots/?rev=121&view=rev
Author: tfoote
Date: 2008-04-17 16:37:45 -0700 (Thu, 17 Apr 2008)
Log Message:
-----------
overloading << operator and adding const correctness checking
Modified Paths:
--------------
pkg/trunk/libTF/simple/Quaternion3D.cc
pkg/trunk/libTF/simple/Quaternion3D.hh
Modified: pkg/trunk/libTF/simple/Quaternion3D.cc
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 22:19:01 UTC (rev 120)
+++ pkg/trunk/libTF/simple/Quaternion3D.cc 2008-04-17 23:37:45 UTC (rev 121)
@@ -60,13 +60,13 @@
} ;
-void Quaternion3D::fromMatrix(NEWMAT::Matrix matIn, unsigned long long time)
+void Quaternion3D::fromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time)
{
// math derived from http://www.j3d.org/matrix_faq/matrfaq_latest.html
Quaternion3DStorage temp;
temp.time = time;
- double * mat = matIn.Store();
+ const double * mat = matIn.Store();
//Get the translations
temp.xt = mat[3];
temp.yt = mat[7];
@@ -200,6 +200,22 @@
};
+Quaternion3D::Quaternion3DStorage& Quaternion3D::Quaternion3DStorage::operator=(const Quaternion3DStorage & input)
+{
+ xt = input.xt;
+ yt = input.yt;
+ zt = input.zt;
+ xr = input.xr;
+ yr = input.yr;
+ zr = input.zr;
+ w = input.w ;
+ time = input.time;
+
+ return *this;
+};
+
+
+
void Quaternion3D::Quaternion3DStorage::Normalize()
{
double mag = getMagnitude();
@@ -214,13 +230,22 @@
return sqrt(xr*xr + yr*yr + zr*zr + w*w);
};
+//Note global scope
+std::ostream & operator<<(std::ostream& mystream, const Quaternion3D::Quaternion3DStorage& storage)
+{
+ mystream << "Storage: " << storage.xt <<", " << storage.yt <<", " << storage.zt<<", " << storage.xr<<", " << storage.yr <<", " << storage.zr <<", " << storage.w<<std::endl;
+ return mystream;
+};
+
+
NEWMAT::Matrix Quaternion3D::asMatrix(unsigned long long time)
{
Quaternion3DStorage temp;
long long diff_time;
getValue(temp, time, diff_time);
+ std::cout << temp;
// printStorage(temp);
// std::cout <<"Locally: "<< xt <<", "<< yt <<", "<< zt <<", "<< xr <<", "<<yr <<", "<<zr <<", "<<w<<std::endl;
//std::cout << "time Difference: "<< diff_time<<std::endl;
@@ -266,7 +291,7 @@
void Quaternion3D::printStorage(const Quaternion3DStorage& storage)
{
- std::cout << "Storage: " << storage.xt <<", " << storage.yt <<", " << storage.zt<<", " << storage.xr<<", " << storage.yr <<", " << storage.zr <<", " << storage.w<<std::endl;
+ std::cout << storage;
};
@@ -310,21 +335,16 @@
};
-void Quaternion3D::add_value(Quaternion3DStorage dataIn)
+void Quaternion3D::add_value(const Quaternion3DStorage &dataIn)
{
- Quaternion3DStorage temp;
- //cout << "started thread" << endl;
-
pthread_mutex_lock(&linked_list_mutex);
insertNode(dataIn);
pruneList();
pthread_mutex_unlock(&linked_list_mutex);
-
-
};
-void Quaternion3D::insertNode(Quaternion3DStorage new_val)
+void Quaternion3D::insertNode(const Quaternion3DStorage & new_val)
{
data_LL* p_current;
data_LL* p_old;
@@ -427,7 +447,7 @@
-int Quaternion3D::findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, unsigned long long target_time, long long &time_diff)
+int Quaternion3D::findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, const unsigned long long target_time, long long &time_diff)
{
data_LL* p_current = first;
@@ -468,7 +488,7 @@
};
-void Quaternion3D::interpolate(Quaternion3DStorage &one, Quaternion3DStorage &two,unsigned long long target_time, Quaternion3DStorage& output)
+void Quaternion3D::interpolate(const Quaternion3DStorage &one, const Quaternion3DStorage &two,unsigned long long target_time, Quaternion3DStorage& output)
{
//fixme do a proper interpolatioln here!!!
output.time = target_time;
@@ -483,7 +503,7 @@
};
-double Quaternion3D::interpolateDouble(double first, unsigned long long first_time, double second, unsigned long long second_time, unsigned long long target_time)
+double Quaternion3D::interpolateDouble(const double first, const unsigned long long first_time, const double second, const unsigned long long second_time, const unsigned long long target_time)
{
if ( first_time == second_time ) {
return first;
Modified: pkg/trunk/libTF/simple/Quaternion3D.hh
===================================================================
--- pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 22:19:01 UTC (rev 120)
+++ pkg/trunk/libTF/simple/Quaternion3D.hh 2008-04-17 23:37:45 UTC (rev 121)
@@ -60,6 +60,7 @@
// Utility functions to normalize and get magnitude.
void Normalize();
double getMagnitude();
+ Quaternion3DStorage & operator=(const Quaternion3DStorage & input);
double xt, yt, zt, xr, yr, zr, w;
unsigned long long time;
@@ -74,7 +75,7 @@
void Set(double _xt, double _yt, double _zt, double _xr, double _yr, double _zr, double _w, unsigned long long time);
//Set the values from a matrix
- void fromMatrix(NEWMAT::Matrix matIn, unsigned long long time);
+ void fromMatrix(const NEWMAT::Matrix& matIn, unsigned long long time);
// Set the values using Euler angles
void fromEuler(double _x, double _y, double _z, double _yaw, double _pitch, double _roll, unsigned long long time);
// Set the values using DH Parameters
@@ -111,26 +112,26 @@
};
bool getValue(Quaternion3DStorage& buff, unsigned long long time, long long &time_diff);
- void add_value(Quaternion3DStorage);//todo fixme finish implementing this
+ void add_value(const Quaternion3DStorage&);//todo fixme finish implementing this
// insert a node into the sorted linked list
- void insertNode(Quaternion3DStorage);
+ void insertNode(const Quaternion3DStorage & );
// prune data older than max_storage_time from the list
void pruneList();
//Find the closest two points in the list
//Return the distance to the closest one
- int findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, unsigned long long target_time, long long &time_diff);
+ int findClosest(Quaternion3DStorage& one, Quaternion3DStorage& two, const unsigned long long target_time, long long &time_diff);
//Interpolate between two nodes and return the interpolated value
// This must always take two valid points!!!!!!
// Only Cpose version implemented
- void interpolate(Quaternion3DStorage &one, Quaternion3DStorage &two, unsigned long long target_time, Quaternion3DStorage& output);
+ void interpolate(const Quaternion3DStorage &one, const Quaternion3DStorage &two, const unsigned long long target_time, Quaternion3DStorage& output);
//Used by interpolate to interpolate between double values
- double interpolateDouble(double, unsigned long long, double, unsigned long long, unsigned long long);
+ double interpolateDouble(const double, const unsigned long long, const double, const unsigned long long, const unsigned long long);
//How long to cache incoming values
unsigned long long max_storage_time;
@@ -148,6 +149,8 @@
+//A global ostream overload for displaying storage
+std::ostream & operator<<(std::ostream& mystream,const Quaternion3D::Quaternion3DStorage & storage);
@@ -156,5 +159,4 @@
-
#endif //QUATERNION3D_HH
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|
