[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [20603] branches/ge_dev: iTaSC: various improvements.
Benoit Bolsee
benoit.bolsee at online.be
Wed Jun 3 17:17:52 CEST 2009
Revision: 20603
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20603
Author: ben2610
Date: 2009-06-03 17:17:52 +0200 (Wed, 03 Jun 2009)
Log Message:
-----------
iTaSC: various improvements. Better cache handling: keep cache if new value identical to values in cache, only write cache on last reiteration on first timestep. Dynamic control of CopyPose gain.
Modified Paths:
--------------
branches/ge_dev/intern/itasc/Armature.cpp
branches/ge_dev/intern/itasc/Armature.hpp
branches/ge_dev/intern/itasc/Cache.cpp
branches/ge_dev/intern/itasc/Cache.hpp
branches/ge_dev/intern/itasc/CopyPose.cpp
branches/ge_dev/intern/itasc/Distance.cpp
branches/ge_dev/intern/itasc/MovingFrame.cpp
branches/ge_dev/intern/itasc/Scene.cpp
branches/ge_dev/intern/itasc/Scene.hpp
branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/ge_dev/intern/itasc/Armature.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.cpp 2009-06-03 15:14:09 UTC (rev 20602)
+++ branches/ge_dev/intern/itasc/Armature.cpp 2009-06-03 15:17:52 UTC (rev 20603)
@@ -13,7 +13,7 @@
namespace iTaSC {
// a joint constraint is characterized by 5 values: tolerance, K, alpha, yd, yddot
-static const unsigned int constraintCacheSize = sizeof(double)*5;
+static const unsigned int constraintCacheSize = 5;
std::string Armature::m_root = "root";
Armature::Armature():
@@ -24,6 +24,7 @@
m_neffector(0),
m_finalized(false),
m_cache(NULL),
+ m_buf(NULL),
m_qCCh(-1),
m_qCTs(0),
m_yCCh(-1),
@@ -48,6 +49,8 @@
if (*it != NULL)
delete (*it);
}
+ if (m_buf)
+ delete [] m_buf;
m_constraints.clear();
}
@@ -56,24 +59,27 @@
m_cache = _cache;
m_qCCh = -1;
m_yCCh = -1;
+ m_buf = NULL;
if (m_cache) {
// add a special channel for the joint
m_qCCh = m_cache->addChannel(this, "q", m_qKdl.rows()*sizeof(double));
// for the constraints, instead of creating many different channels, we will
// create a single channel for all the constraints
if (m_nconstraint) {
- m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize);
+ m_yCCh = m_cache->addChannel(this, "y", m_nconstraint*constraintCacheSize*sizeof(double));
}
+ if (m_nconstraint)
+ m_buf = new double[m_nconstraint*constraintCacheSize];
// store the initial cache position at timestamp 0
pushQ(0);
pushConstraints(0);
}
}
-
void Armature::pushQ(CacheTS timestamp)
{
if (m_qCCh >= 0) {
- m_cache->addCacheItem(this, m_qCCh, timestamp, &m_qKdl(0), m_qKdl.rows()*sizeof(double));
+ // try to keep the cache if the joints are the same
+ m_cache->addCacheVectorIfDifferent(this, m_qCCh, timestamp, &m_qKdl(0), m_qKdl.rows(), KDL::epsilon);
m_qCTs = timestamp;
}
}
@@ -99,8 +105,9 @@
void Armature::pushConstraints(CacheTS timestamp)
{
if (m_yCCh >= 0) {
- double *item = (double*)m_cache->addCacheItem(this, m_yCCh, timestamp, NULL, m_nconstraint*constraintCacheSize);
- if (item) {
+ double *buf = NULL;
+ if (m_nconstraint) {
+ double *item = m_buf;
for (unsigned int i=0; i<m_nconstraint; i++) {
JointConstraint_struct* pConstraint = m_constraints[i];
*item++ = pConstraint->values.feedback;
@@ -110,6 +117,7 @@
*item++ = pConstraint->values.alpha;
}
}
+ m_cache->addCacheVectorIfDifferent(this, m_yCCh, timestamp, m_buf, m_nconstraint*constraintCacheSize, KDL::epsilon);
m_yCTs = timestamp;
}
}
@@ -324,7 +332,7 @@
m_qdotKdl(i)=m_qdot(i)*timestamp.realTimestep;
Add(m_qKdl,m_qdotKdl,m_qKdl);
- if (!timestamp.substep) {
+ if (!timestamp.substep && timestamp.cache) {
pushQ(timestamp.cacheTimestamp);
pushConstraints(timestamp.cacheTimestamp);
}
Modified: branches/ge_dev/intern/itasc/Armature.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.hpp 2009-06-03 15:14:09 UTC (rev 20602)
+++ branches/ge_dev/intern/itasc/Armature.hpp 2009-06-03 15:17:52 UTC (rev 20603)
@@ -93,6 +93,7 @@
unsigned int m_neffector;
bool m_finalized;
Cache* m_cache;
+ double *m_buf;
int m_qCCh;
CacheTS m_qCTs;
int m_yCCh;
Modified: branches/ge_dev/intern/itasc/Cache.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Cache.cpp 2009-06-03 15:14:09 UTC (rev 20602)
+++ branches/ge_dev/intern/itasc/Cache.cpp 2009-06-03 15:17:52 UTC (rev 20603)
@@ -9,6 +9,8 @@
#include <malloc.h>
#include "Cache.hpp"
+#include <math.h>
+
namespace iTaSC {
CacheEntry::~CacheEntry()
@@ -457,7 +459,7 @@
return CACHE_ITEM_DATA_POINTER(item);
}
-const void *Cache::getCurrentCacheItem(const void *device, int id, unsigned int timestamp)
+const CacheItem *Cache::getCurrentCacheItemInternal(const void *device, int id, CacheTS timestamp)
{
CacheMap::iterator it = m_cache.find(device);
CacheEntry *entry;
@@ -477,9 +479,35 @@
return NULL;
if (buffer->m_firstTimestamp+item->m_timeOffset != timestamp)
return NULL;
- return CACHE_ITEM_DATA_POINTER(item);
+ return item;
}
+const void *Cache::getCurrentCacheItem(const void *device, int channel, unsigned int timestamp)
+{
+ const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp);
+ return (item) ? CACHE_ITEM_DATA_POINTER(item) : NULL;
+}
+double *Cache::addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *newdata, unsigned int length, double threshold)
+{
+ const CacheItem *item = getCurrentCacheItemInternal(device, channel, timestamp);
+ unsigned int sizeW = CACHE_ITEM_SIZEW(item,length*sizeof(double));
+ if (!item || item->m_sizeW != sizeW)
+ return (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double));
+ double *olddata = (double*)CACHE_ITEM_DATA_POINTER(item);
+ if (!length)
+ return olddata;
+ double *ref = olddata;
+ double *v = newdata;
+ unsigned int i;
+ for (i=length; i>0; --i) {
+ if (fabs(*v-*ref) > threshold)
+ break;
+ *ref++ = *v++;
+ }
+ if (i)
+ olddata = (double*)addCacheItem(device, channel, timestamp, newdata, length*sizeof(double));
+ return olddata;
+}
}
Modified: branches/ge_dev/intern/itasc/Cache.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Cache.hpp 2009-06-03 15:14:09 UTC (rev 20602)
+++ branches/ge_dev/intern/itasc/Cache.hpp 2009-06-03 15:17:52 UTC (rev 20603)
@@ -22,7 +22,7 @@
/* macro to get item data position, item=CacheItem pointer */
#define CACHE_ITEM_DATA_POINTER(item) (void*)((unsigned char*)item+sizeof(CacheItem)+CACHE_ITEM_GAPB(item))
/* macro to get item size in 32bit words from item address and length, item=CacheItem pointer */
-#define CACHE_ITEM_SIZEW(item,length) (unsigned int)((sizeof(CacheItem)+CACHE_ITEM_GAPB(item)+((length+3)&~0x3))>>2)
+#define CACHE_ITEM_SIZEW(item,length) (unsigned int)((sizeof(CacheItem)+CACHE_ITEM_GAPB(item)+(((length)+3)&~0x3))>>2)
/* macto to move from one item to the next, item=CacheItem pointer, updated by the macro */
#define CACHE_NEXT_ITEM(item) ((item)+(item)->m_sizeW)
#define CACHE_BLOCK_ITEM_ADDR(chan,buf,block) (&(buf)->m_firstItem+(((unsigned int)(block)<<chan->m_positionToBlockShiftW)+(buf)->lookup[block].m_offsetW))
@@ -38,7 +38,8 @@
CacheTS cacheTimestamp;
unsigned int substep:1;
unsigned int reiterate:1;
- unsigned int dummy:30;
+ unsigned int cache:1;
+ unsigned int dummy:29;
};
/* utility function to return second timestamp to millisecond */
@@ -74,6 +75,10 @@
*/
struct CacheEntry;
+struct CacheBuffer;
+struct CacheItem;
+struct CacheChannel;
+
class Cache
{
private:
@@ -81,6 +86,7 @@
Dynamically updated when more devices create cache channels */
typedef std::map<const void *, struct CacheEntry*> CacheMap;
CacheMap m_cache;
+ const CacheItem *getCurrentCacheItemInternal(const void *device, int channel, CacheTS timestamp);
public:
Cache();
@@ -103,6 +109,13 @@
return: error: NULL, success: pointer to item in cache */
void *addCacheItem(const void *device, int channel, CacheTS timestamp, void *data, unsigned int length);
+ /* specialized function to add a vector of double in the cache
+ It will first check if a vector exist already in the cache for the same timestamp
+ and compared the cached vector with the new values.
+ If all values are within threshold, the vector is updated but the cache is not deleted
+ for the future timestamps. */
+ double *addCacheVectorIfDifferent(const void *device, int channel, CacheTS timestamp, double *data, unsigned int length, double threshold);
+
/* returns the cache item with timestamp that is just before the given timestamp.
returns the data pointer or NULL if there is no cache item before timestamp.
On return, timestamp is updated with the actual timestamp of the item being returned.
@@ -120,7 +133,6 @@
/* the following structures are not internal use only, they should not be used directly */
-struct CacheChannel;
struct CacheEntry
{
CacheChannel *m_channelArray; // array of channels, automatically resized if more channels are created
@@ -129,8 +141,6 @@
~CacheEntry();
};
-struct CacheBuffer;
-struct CacheItem;
struct CacheChannel
{
struct CacheBuffer *m_firstBuffer; // first buffer of list
Modified: branches/ge_dev/intern/itasc/CopyPose.cpp
===================================================================
--- branches/ge_dev/intern/itasc/CopyPose.cpp 2009-06-03 15:14:09 UTC (rev 20602)
+++ branches/ge_dev/intern/itasc/CopyPose.cpp 2009-06-03 15:17:52 UTC (rev 20603)
@@ -14,6 +14,7 @@
namespace iTaSC
{
+const unsigned int maxPoseCacheSize = (2*(3+3*2));
CopyPose::CopyPose(unsigned int control_output, unsigned int dynamic_output, double accuracy, unsigned int maximum_iterations):
ConstraintSet(),
m_cache(NULL),
@@ -105,7 +106,7 @@
}
assert(_nc == m_nc);
m_Jf=e_identity_matrix(6,6);
- m_poseCacheSize = sizeof(double)*(((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0));
+ m_poseCacheSize = ((nrotCache)?(3+nrotCache*2):0)+((nposCache)?(3+nposCache*2):0);
}
CopyPose::~CopyPose()
@@ -131,7 +132,7 @@
m_poseCCh = -1;
if (m_cache) {
// create one channel for the coordinates
- m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize);
+ m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize*sizeof(double));
// don't save initial value, it will be recomputed from external pose
//pushPose(0);
}
@@ -162,13 +163,16 @@
void CopyPose::pushPose(CacheTS timestamp)
{
if (m_poseCCh >= 0) {
- double *item = (double*)m_cache->addCacheItem(this, m_poseCCh, timestamp, NULL, m_poseCacheSize);
- if (item) {
+ if (m_poseCacheSize) {
+ double buf[maxPoseCacheSize];
+ double *item = buf;
if (m_outputDynamic & CTL_POSITION)
item = pushValues(item, &m_pos, CTL_POSITIONX);
if (m_outputDynamic & CTL_ROTATION)
item = pushValues(item, &m_rot, CTL_ROTATIONX);
- }
+ m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, buf, m_poseCacheSize, KDL::epsilon);
+ } else
+ m_cache->addCacheVectorIfDifferent(this, m_poseCCh, timestamp, NULL, 0, KDL::epsilon);
m_poseCTs = timestamp;
}
}
@@ -264,7 +268,7 @@
interpolateOutput(&m_pos, CTL_POSITIONX, timestamp);
if (m_outputDynamic & CTL_ROTATION)
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list