[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [21152] branches/ge_dev: iTaSC: implement 2DoF Swing joint, connect iTaSC cache reset to BKE point cache.

Benoit Bolsee benoit.bolsee at online.be
Thu Jun 25 13:57:19 CEST 2009


Revision: 21152
          http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=21152
Author:   ben2610
Date:     2009-06-25 13:57:19 +0200 (Thu, 25 Jun 2009)

Log Message:
-----------
iTaSC: implement 2DoF Swing joint, connect iTaSC cache reset to BKE point cache. Fix bug in KDL on Vector2 Norm. Add Helper functions GetValue to get vector coordinates from KDL.

Modified Paths:
--------------
    branches/ge_dev/intern/itasc/Armature.cpp
    branches/ge_dev/intern/itasc/Cache.cpp
    branches/ge_dev/intern/itasc/Cache.hpp
    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/joint.cpp
    branches/ge_dev/intern/itasc/kdl/joint.hpp
    branches/ge_dev/intern/itasc/kdl/segment.cpp
    branches/ge_dev/intern/itasc/kdl/segment.hpp
    branches/ge_dev/intern/itasc/kdl/treejnttojacsolver.cpp
    branches/ge_dev/source/blender/blenkernel/intern/pointcache.c
    branches/ge_dev/source/blender/ikplugin/BIK_api.h
    branches/ge_dev/source/blender/ikplugin/intern/ikplugin_api.c
    branches/ge_dev/source/blender/ikplugin/intern/ikplugin_api.h
    branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
    branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.h

Modified: branches/ge_dev/intern/itasc/Armature.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.cpp	2009-06-25 10:52:09 UTC (rev 21151)
+++ branches/ge_dev/intern/itasc/Armature.cpp	2009-06-25 11:57:19 UTC (rev 21152)
@@ -364,11 +364,18 @@
 	for (unsigned int q_nr=0; q_nr<m_nq; ) {
 		Joint_struct& joint = m_joints[q_nr];
 		switch (joint.type) {
+		case KDL::Joint::Swing:
+			{
+				double* qdot=&m_qdotKdl(q_nr);
+				double* q=&m_qKdl(q_nr);
+				(KDL::Rot(KDL::Vector(q[0],0.0,q[1]))*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(q);
+				break;
+			}
 		case KDL::Joint::Sphere:
 			{
 				double* qdot=&m_qdotKdl(q_nr);
 				double* q=&m_qKdl(q_nr);
-				(KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)*KDL::Rot(KDL::Vector(q))).GetRot().GetValue(q);
+				(KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(q);
 				break;
 			}
 		default:

Modified: branches/ge_dev/intern/itasc/Cache.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Cache.cpp	2009-06-25 10:52:09 UTC (rev 21151)
+++ branches/ge_dev/intern/itasc/Cache.cpp	2009-06-25 11:57:19 UTC (rev 21152)
@@ -305,6 +305,83 @@
 	return 0;
 }
 
+void Cache::clearCacheFrom(const void *device, CacheTS timestamp)
+{
+	CacheMap::iterator it = (device) ? m_cache.find(device) : m_cache.begin();
+	CacheEntry *entry;
+	CacheChannel *channel;
+	CacheBuffer *buffer, *nextBuffer, *prevBuffer;
+	CacheItem *item, *prevItem, *nextItem;
+	unsigned int positionW, block;
+
+	while (it != m_cache.end()) {
+		entry = it->second;
+		for (unsigned int ch=0; ch<entry->m_count; ch++) {
+			channel = &entry->m_channelArray[ch];
+			if (channel->m_busy) {
+				item = channel->findItemOrLater(timestamp, &buffer);
+				if (item ) {
+					// this item and all later items will be removed, clear any later buffer
+					while ((nextBuffer = buffer->m_next) != NULL) {
+						buffer->m_next = nextBuffer->m_next;
+						free(nextBuffer);
+					}
+					positionW = CACHE_ITEM_POSITIONW(buffer,item);
+					if (positionW == 0) {
+						// this item is the first one of the buffer, remove the buffer completely
+						// first find the buffer just before it
+						nextBuffer = channel->m_firstBuffer;
+						prevBuffer = NULL;
+						while (nextBuffer != buffer) {
+							prevBuffer = nextBuffer;
+							nextBuffer = nextBuffer->m_next;
+							// we must quit this loop before reaching the end of the list
+							assert(nextBuffer);
+						}
+						free(buffer);
+						buffer = prevBuffer;
+						if (buffer == NULL)
+							// this was also the first buffer
+							channel->m_firstBuffer = NULL;
+					} else {
+						// removing this item means finding the previous item to make it the last one
+						block = positionW>>channel->m_positionToBlockShiftW;
+						if (block == 0) {
+							// start from first item, we know it is not our item because positionW > 0
+							prevItem = &buffer->m_firstItem;
+						} else {
+							// no need to check the current block, it will point to our item or a later one
+							// but the previous block will be a good start for sure.
+							block--;
+							prevItem = CACHE_BLOCK_ITEM_ADDR(channel,buffer,block);
+						}
+						while ((nextItem = CACHE_NEXT_ITEM(prevItem)) < item)
+							prevItem = nextItem;
+						// we must have found our item
+						assert(nextItem==item);
+						// now set the buffer
+						buffer->m_lastItemPositionW = CACHE_ITEM_POSITIONW(buffer,prevItem);
+						buffer->m_firstFreePositionW = positionW;
+						buffer->m_lastTimestamp = buffer->m_firstTimestamp + prevItem->m_timeOffset;
+						block = buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW;
+						buffer->lookup[block].m_offsetW = buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW;
+						buffer->lookup[block].m_timeOffset = prevItem->m_timeOffset;
+					}
+					// set the channel
+					channel->m_lastBuffer = buffer;
+					if (buffer) {
+						channel->m_lastTimestamp = buffer->m_lastTimestamp;
+						channel->m_lastItemPositionW = buffer->m_lastItemPositionW;
+					}
+				}
+			}
+		}
+		if (device)
+			break;
+		++it;
+	}
+}
+
 void *Cache::addCacheItem(const void *device, int id, unsigned int timestamp, void *data, unsigned int length)
 {
 	CacheMap::iterator it = m_cache.find(device);

Modified: branches/ge_dev/intern/itasc/Cache.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Cache.hpp	2009-06-25 10:52:09 UTC (rev 21151)
+++ branches/ge_dev/intern/itasc/Cache.hpp	2009-06-25 11:57:19 UTC (rev 21152)
@@ -101,6 +101,9 @@
 	int deleteChannel(const void *device, int channel);
 	/* delete all channels of a device and remove the device from the map */
 	int deleteDevice(const void *device);
+	/* removes all cache items, leaving the special item at timestamp=0. 
+	   if device=NULL, apply to all devices. */
+	void clearCacheFrom(const void *device, CacheTS timestamp);
 
 	/* add a new cache item
 	   channel: the cache channel (as returned by AddChannel

Modified: branches/ge_dev/intern/itasc/kdl/frames.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.cpp	2009-06-25 10:52:09 UTC (rev 21151)
+++ branches/ge_dev/intern/itasc/kdl/frames.cpp	2009-06-25 11:57:19 UTC (rev 21152)
@@ -83,10 +83,14 @@
 
 double Vector2::Norm() const
 {
-    if (fabs(data[0]) > fabs(data[1]) ) {
-        return data[0]*sqrt(1+sqr(data[1]/data[0]));
+    double tmp0 = fabs(data[0]);
+    double tmp1 = fabs(data[1]);
+    if (tmp0 >= tmp1) {
+		if (tmp1 == 0)
+			return 0;
+        return tmp0*sqrt(1+sqr(tmp1/tmp0));
     } else {
-        return data[1]*sqrt(1+sqr(data[0]/data[1]));
+        return tmp1*sqrt(1+sqr(tmp0/tmp1));
     }
 }
 // makes v a unitvector and returns the norm of v.
@@ -320,6 +324,18 @@
        return axis * alfa;
      }
 
+Vector2 Rotation::GetXZRot() const
+{
+	// [0,1,0] x Y
+	Vector2 axis(data[7], -data[1]);
+	double norm = axis.Normalize();
+	if (norm < epsilon) {
+		norm = (data[4] < 0.0) ? PI : 0.0;
+	} else {
+		norm = acos(data[4]);
+	}
+	return axis*norm;
+}
 
 
 /** Returns the rotation angle around the equiv. axis

Modified: branches/ge_dev/intern/itasc/kdl/frames.hpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.hpp	2009-06-25 10:52:09 UTC (rev 21151)
+++ branches/ge_dev/intern/itasc/kdl/frames.hpp	2009-06-25 11:57:19 UTC (rev 21152)
@@ -370,6 +370,10 @@
     //! and its norm is angle
     Vector GetRot() const;
 
+    //! Returns a 2D vector representing the equivalent rotation in the XZ plane that brings the
+    //! Y axis onto the Matrix Y axis and its norm is angle
+    Vector2 GetXZRot() const;
+
 	/** Returns the rotation angle around the equiv. axis
 	 * @param axis the rotation axis is returned in this variable
 	 * @param eps :  in the case of angle == 0 : rot axis is undefined and choosen
@@ -917,6 +921,9 @@
      //! Access to elements, range checked when NDEBUG is not set, from 0..1
      inline double& operator() (int index);
 
+	 //! store vector components in array
+	 inline void GetValue(double* xy);
+
      inline void ReverseSign();
      inline Vector2& operator-=(const Vector2& arg);
      inline Vector2& operator +=(const Vector2& arg);

Modified: branches/ge_dev/intern/itasc/kdl/frames.inl
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.inl	2009-06-25 10:52:09 UTC (rev 21151)
+++ branches/ge_dev/intern/itasc/kdl/frames.inl	2009-06-25 11:57:19 UTC (rev 21152)
@@ -757,6 +757,10 @@
     return *this;
 }
 
+IMETHOD void Vector2::GetValue(double* xy)
+{
+        xy[0]=data[0];xy[1]=data[1];
+}
 
 IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs)
 {

Modified: branches/ge_dev/intern/itasc/kdl/joint.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/joint.cpp	2009-06-25 10:52:09 UTC (rev 21151)
+++ branches/ge_dev/intern/itasc/kdl/joint.cpp	2009-06-25 11:57:19 UTC (rev 21152)
@@ -27,21 +27,11 @@
                  const double& _inertia, const double& _damping, const double& _stiffness):
         type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
     {
-		// this constructor should not be used for sphere joint, assume no offset in basis
+		// for sphere and swing, offset is not used, assume no offset
     }
 
-    Joint::Joint(const JointType& _type, const double& _scale, const Rotation& _basis,
-                 const double& _inertia, const double& _damping, const double& _stiffness):
-        type(_type),scale(_scale),offset(0.0),inertia(_inertia),damping(_damping),stiffness(_stiffness)
-    {
-		// this constructor should not be used for 1DOF joint
-		if (_type == Sphere) {
-			basis = _basis;
-		}
-    }
-
     Joint::Joint(const Joint& in):
-	type(in.type),scale(in.scale),offset(in.offset),basis(in.basis),
+	type(in.type),scale(in.scale),offset(in.offset),
         inertia(in.inertia),damping(in.damping),stiffness(in.stiffness)
     {
     }
@@ -51,7 +41,6 @@
         type=in.type;
         scale=in.scale;
         offset=in.offset;
-		basis=in.basis;
         inertia=in.inertia;
         damping=in.damping;
         stiffness=in.stiffness;
@@ -88,8 +77,13 @@
 		case Sphere:
 			// the joint angles represent a rotation vector expressed in the base frame of the joint
 			// (= the frame you get when there is no offset nor rotation)
-			return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2]))*basis);
+			return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2])));
 			break;
+		case Swing:
+			// the joint angles represent a 2D rotation vector in the XZ planee of the base frame of the joint
+			// (= the frame you get when there is no offset nor rotation)
+			return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1])));
+			break;
         default:
             return Frame::Identity();
             break;
@@ -117,6 +111,14 @@
         case TransZ:
             return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
             break;
+		case Swing:
+			switch (dof) {
+			case 0:
+				return Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+			case 1:
+				return Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+			}
+            return Twist::Zero();
 		case Sphere:

@@ Diff output truncated at 10240 characters. @@




More information about the Bf-blender-cvs mailing list