[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [20455] branches/ge_dev/intern/itasc/kdl: Add support in KDL for easy matrix conversion.
Benoit Bolsee
benoit.bolsee at online.be
Wed May 27 19:18:15 CEST 2009
Revision: 20455
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20455
Author: ben2610
Date: 2009-05-27 19:18:14 +0200 (Wed, 27 May 2009)
Log Message:
-----------
Add support in KDL for easy matrix conversion.
Modified Paths:
--------------
branches/ge_dev/intern/itasc/kdl/frames.cpp
branches/ge_dev/intern/itasc/kdl/frames.hpp
branches/ge_dev/intern/itasc/kdl/frames.inl
branches/ge_dev/intern/itasc/kdl/treefksolver.hpp
branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.cpp
branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.hpp
Modified: branches/ge_dev/intern/itasc/kdl/frames.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.cpp 2009-05-27 15:41:44 UTC (rev 20454)
+++ branches/ge_dev/intern/itasc/kdl/frames.cpp 2009-05-27 17:18:14 UTC (rev 20455)
@@ -159,8 +159,14 @@
Equal(a.data[8],b.data[8],eps) );
}
+void Rotation::Ortho()
+{
+ double n;
+ n=sqrt(sqr(data[0])+sqr(data[3])+sqr(data[6]));n=(n>1e-10)?1.0/n:0.0;data[0]*=n;data[3]*=n;data[6]*=n;
+ n=sqrt(sqr(data[1])+sqr(data[4])+sqr(data[7]));n=(n>1e-10)?1.0/n:0.0;data[1]*=n;data[4]*=n;data[7]*=n;
+ n=sqrt(sqr(data[2])+sqr(data[5])+sqr(data[8]));n=(n>1e-10)?1.0/n:0.0;data[2]*=n;data[5]*=n;data[8]*=n;
+}
-
Rotation operator *(const Rotation& lhs,const Rotation& rhs)
// Complexity : 27M+27A
{
Modified: branches/ge_dev/intern/itasc/kdl/frames.hpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.hpp 2009-05-27 15:41:44 UTC (rev 20454)
+++ branches/ge_dev/intern/itasc/kdl/frames.hpp 2009-05-27 17:18:14 UTC (rev 20455)
@@ -295,6 +295,8 @@
inline Rotation(const Vector& x,const Vector& y,const Vector& z);
// default copy constructor is sufficient
+ inline void setValue(float* oglmat);
+ inline void getValue(float* oglmat);
inline Rotation& operator=(const Rotation& arg);
@@ -354,6 +356,9 @@
//! Along an arbitrary axes. rotvec should be normalized.
static Rotation Rot2(const Vector& rotvec,double angle);
+
+ // make sure the matrix is a pure rotation (no scaling)
+ void Ortho();
//! Returns a vector with the direction of the equiv. axis
//! and its norm is angle
@@ -514,6 +519,9 @@
//! The position matrix defaults to zero
explicit inline Frame(const Rotation& R);
+ inline void setValue(float* oglmat);
+ inline void getValue(float* oglmat);
+
inline Frame() {}
//! The copy constructor. Normal copy by value semantics.
inline Frame(const Frame& arg);
@@ -530,7 +538,7 @@
//! Access to elements 0..3,0..3, bounds are checked when NDEBUG is not set
inline double operator() (int i,int j) const;
-// = Inverse
+ // = Inverse
//! Gives back inverse transformation of a Frame
inline Frame Inverse() const;
Modified: branches/ge_dev/intern/itasc/kdl/frames.inl
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.inl 2009-05-27 15:41:44 UTC (rev 20454)
+++ branches/ge_dev/intern/itasc/kdl/frames.inl 2009-05-27 17:18:14 UTC (rev 20455)
@@ -626,7 +626,22 @@
);
}
+void Rotation::setValue(float* oglmat)
+{
+ data[0] = *oglmat++; data[3] = *oglmat++; data[6] = *oglmat++; oglmat++;
+ data[1] = *oglmat++; data[4] = *oglmat++; data[7] = *oglmat++; oglmat++;
+ data[2] = *oglmat++; data[5] = *oglmat++; data[8] = *oglmat;
+ Ortho();
+}
+void Rotation::getValue(float* oglmat)
+{
+ *oglmat++ = (float)data[0]; *oglmat++ = (float)data[3]; *oglmat++ = (float)data[6]; *oglmat++ = 0.f;
+ *oglmat++ = (float)data[1]; *oglmat++ = (float)data[4]; *oglmat++ = (float)data[7]; *oglmat++ = 0.f;
+ *oglmat++ = (float)data[2]; *oglmat++ = (float)data[5]; *oglmat++ = (float)data[8]; *oglmat++ = 0.f;
+ *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat++ = 0.f; *oglmat = 1.f;
+}
+
void Rotation::SetInverse()
{
double tmp;
@@ -679,7 +694,21 @@
}
+void Frame::setValue(float* oglmat)
+{
+ M.setValue(oglmat);
+ p.data[0] = oglmat[12];
+ p.data[1] = oglmat[13];
+ p.data[2] = oglmat[14];
+}
+void Frame::getValue(float* oglmat)
+{
+ M.getValue(oglmat);
+ oglmat[12] = (float)p.data[0];
+ oglmat[13] = (float)p.data[1];
+ oglmat[14] = (float)p.data[2];
+}
void Vector::Set2DPlane(const Frame& F_someframe_XY,const Vector2& v_XY)
// a 3D vector where the 2D vector v is put in the XY plane of the frame
Modified: branches/ge_dev/intern/itasc/kdl/treefksolver.hpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/treefksolver.hpp 2009-05-27 15:41:44 UTC (rev 20454)
+++ branches/ge_dev/intern/itasc/kdl/treefksolver.hpp 2009-05-27 17:18:14 UTC (rev 20455)
@@ -53,7 +53,7 @@
*
* @return if < 0 something went wrong
*/
- virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)=0;
+ virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)=0;
virtual ~TreeFkSolverPos(){};
};
Modified: branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.cpp 2009-05-27 15:41:44 UTC (rev 20454)
+++ branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.cpp 2009-05-27 17:18:14 UTC (rev 20455)
@@ -30,35 +30,36 @@
{
}
- int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName)
+ int TreeFkSolverPos_recursive::JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName)
{
SegmentMap::const_iterator it = tree.getSegment(segmentName);
-
+ SegmentMap::const_iterator baseit = tree.getSegment(baseName);
if(q_in.rows() != tree.getNrOfJoints())
return -1;
else if(it == tree.getSegments().end()) //if the segment name is not found
return -2;
+ else if(baseit == tree.getSegments().end()) //if the base segment name is not found
+ return -3;
else{
const TreeElement& currentElement = it->second;
- p_out = recursiveFk(q_in, it);
+ p_out = recursiveFk(q_in, it, baseit);
return 0;
}
}
- Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it)
+ Frame TreeFkSolverPos_recursive::recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit)
{
//gets the frame for the current element (segment)
const TreeElement& currentElement = it->second;
- Frame currentFrame = currentElement.segment.pose(q_in(currentElement.q_nr));
- SegmentMap::const_iterator rootIterator = tree.getSegment("root");
- if(it == rootIterator){
- return currentFrame;
+ if(it == baseit){
+ return KDL::Frame::Identity();
}
else{
+ Frame currentFrame = currentElement.segment.pose(q_in(currentElement.q_nr));
SegmentMap::const_iterator parentIt = currentElement.parent;
- return recursiveFk(q_in, parentIt) * currentFrame;
+ return recursiveFk(q_in, parentIt, baseit) * currentFrame;
}
}
Modified: branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.hpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.hpp 2009-05-27 15:41:44 UTC (rev 20454)
+++ branches/ge_dev/intern/itasc/kdl/treefksolverpos_recursive.hpp 2009-05-27 17:18:14 UTC (rev 20455)
@@ -40,12 +40,12 @@
TreeFkSolverPos_recursive(const Tree& tree);
~TreeFkSolverPos_recursive();
- virtual int JntToCart(const JntArray& q_in, Frame& p_out, std::string segmentName);
+ virtual int JntToCart(const JntArray& q_in, Frame& p_out, const std::string& segmentName, const std::string& baseName);
private:
const Tree tree;
- Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it);
+ Frame recursiveFk(const JntArray& q_in, const SegmentMap::const_iterator& it, const SegmentMap::const_iterator& baseit);
};
}
More information about the Bf-blender-cvs
mailing list