[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [20582] branches/ge_dev: iTaSC: various bug fix.
Benoit Bolsee
benoit.bolsee at online.be
Tue Jun 2 19:05:20 CEST 2009
Revision: 20582
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20582
Author: ben2610
Date: 2009-06-02 19:05:19 +0200 (Tue, 02 Jun 2009)
Log Message:
-----------
iTaSC: various bug fix. Don't store MovingFrame initial pose in the cache: it creates unnecessary large velocity on first iteration. Adapt CopyPose in consequence. Decompose spherical joint in RX+RY+RZ to get same behavior as iksolver. Fix bug in auto timestep causing slow convergence.
Modified Paths:
--------------
branches/ge_dev/intern/itasc/CopyPose.cpp
branches/ge_dev/intern/itasc/CopyPose.hpp
branches/ge_dev/intern/itasc/MovingFrame.cpp
branches/ge_dev/intern/itasc/Scene.cpp
branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
Modified: branches/ge_dev/intern/itasc/CopyPose.cpp
===================================================================
--- branches/ge_dev/intern/itasc/CopyPose.cpp 2009-06-02 16:40:15 UTC (rev 20581)
+++ branches/ge_dev/intern/itasc/CopyPose.cpp 2009-06-02 17:05:19 UTC (rev 20582)
@@ -132,8 +132,8 @@
if (m_cache) {
// create one channel for the coordinates
m_poseCCh = m_cache->addChannel(this, "Xf", m_poseCacheSize);
- // save initial constraint in cache position 0
- pushPose(0);
+ // don't save initial value, it will be recomputed from external pose
+ //pushPose(0);
}
}
@@ -198,33 +198,40 @@
return item;
}
-bool CopyPose::popPose(CacheTS timestamp)
+bool CopyPose::popPose(CacheTS timestamp, bool& found)
{
+ found = false;
if (m_poseCCh >= 0) {
double *item = (double*)m_cache->getPreviousCacheItem(this, m_poseCCh, ×tamp);
- if (item && timestamp != m_poseCTs) {
- int i=0;
- if (m_outputControl & CTL_POSITION) {
- if (m_outputDynamic & CTL_POSITION) {
- item = restoreValues(item, &m_values[i], &m_pos, CTL_POSITIONX);
+ if (item) {
+ found = true;
+ if (timestamp != m_poseCTs) {
+ int i=0;
+ if (m_outputControl & CTL_POSITION) {
+ if (m_outputDynamic & CTL_POSITION) {
+ item = restoreValues(item, &m_values[i], &m_pos, CTL_POSITIONX);
+ }
+ i++;
}
- i++;
- }
- if (m_outputControl & CTL_ROTATION) {
- if (m_outputDynamic & CTL_ROTATION) {
- item = restoreValues(item, &m_values[i], &m_rot, CTL_ROTATIONX);
+ if (m_outputControl & CTL_ROTATION) {
+ if (m_outputDynamic & CTL_ROTATION) {
+ item = restoreValues(item, &m_values[i], &m_rot, CTL_ROTATIONX);
+ }
+ i++;
}
- i++;
+ m_poseCTs = timestamp;
+ item = NULL;
}
- // The time is not contiguous, should also load the joint.
+ }
+ if (!item) {
+ // The time is not contiguous or cache is empty, should also load the joint.
// But this constraint has no joint, only a full rotation matrix that we don't save.
// We must get the pose from the scene using the callback. Hopefully the constraint
// are updated after the objects, so we can trust the object position
getExternalPose(m_internalPose);
updateJacobian();
- m_poseCTs = timestamp;
return true;
- }
+ }
}
return false;
}
@@ -402,9 +409,10 @@
{
//IMO this should be done, no idea if it is enough (wrt Distance impl)
Twist y = diff(m_internalPose,F_identity);
+ bool found = true;
if (!timestamp.substep) {
if (!timestamp.reiterate) {
- if (popPose(timestamp.cacheTimestamp))
+ if (popPose(timestamp.cacheTimestamp, found))
// pose updated, recalculate the rotation
y = diff(m_internalPose,F_identity);
}
@@ -418,7 +426,7 @@
updateValues(y.rot, &m_values[i++], &m_rot, CTL_ROTATIONX);
}
if ((*m_constraintCallback)(timestamp, m_values, m_nvalues, m_constraintParam)) {
- setControlParameters(m_values, m_nvalues, timestamp.realTimestep);
+ setControlParameters(m_values, m_nvalues, (found)?timestamp.realTimestep:0.0);
}
}
}
Modified: branches/ge_dev/intern/itasc/CopyPose.hpp
===================================================================
--- branches/ge_dev/intern/itasc/CopyPose.hpp 2009-06-02 16:40:15 UTC (rev 20581)
+++ branches/ge_dev/intern/itasc/CopyPose.hpp 2009-06-02 17:05:19 UTC (rev 20582)
@@ -81,7 +81,7 @@
} m_rot, m_pos;
void pushPose(CacheTS timestamp);
- bool popPose(CacheTS timestamp);
+ bool popPose(CacheTS timestamp, bool& found);
int nBitsOn(unsigned int v)
{ int n=0; while(v) { if (v&1) n++; v>>=1; } return n; }
double* restoreValues(double* item, ConstraintValues* _values, ControlState* _state, unsigned int mask);
Modified: branches/ge_dev/intern/itasc/MovingFrame.cpp
===================================================================
--- branches/ge_dev/intern/itasc/MovingFrame.cpp 2009-06-02 16:40:15 UTC (rev 20581)
+++ branches/ge_dev/intern/itasc/MovingFrame.cpp 2009-06-02 17:05:19 UTC (rev 20582)
@@ -36,8 +36,8 @@
m_poseCCh = -1;
if (m_cache) {
m_poseCCh = m_cache->addChannel(this,"pose",frameCacheSize);
- // add the initial position at timestamp 0
- pushInternalFrame(0);
+ // don't store the initial pose, it's causing unnecessary large velocity on the first step
+ //pushInternalFrame(0);
}
}
Modified: branches/ge_dev/intern/itasc/Scene.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Scene.cpp 2009-06-02 16:40:15 UTC (rev 20581)
+++ branches/ge_dev/intern/itasc/Scene.cpp 2009-06-02 17:05:19 UTC (rev 20582)
@@ -217,6 +217,7 @@
return false;
Timestamp ts;
ts.realTimestamp = timestamp;
+ // initially we start with the full timestep to allow velocity estimation over the full interval
ts.realTimestep = timestep;
setCacheTimestamp(ts);
ts.substep = 0;
@@ -399,15 +400,14 @@
}
}
if (numsubstep > 1) {
- // change timestep so that for next substep, updateControlOutput() will have
- // the correct timestep (although it is not using it)
- ts.realTimestep = timesubstep;
ts.substep = 1;
} else {
// set substep to false for last iteration so that controlled output
// can be updated in updateKinematics() and model_update)() before next call to Secne::update()
ts.substep = 0;
}
+ // change timestep so that integration is done correctly
+ ts.realTimestep = timesubstep;
//Update the Objects
for(ObjectMap::iterator it=objects.begin();it!=objects.end();++it){
Modified: branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-02 16:40:15 UTC (rev 20581)
+++ branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-06-02 17:05:19 UTC (rev 20582)
@@ -376,6 +376,19 @@
}
}
+static void GetEulerXYZ(const KDL::Rotation& R, double& X,double& Y,double& Z)
+{
+ if (fabs(R(0,2)) > 1.0 - KDL::epsilon ) {
+ X = KDL::sign(R(0,2)) * KDL::atan2(-R(1,0), R(1,1));
+ Y = KDL::sign(R(0,2)) * KDL::PI / 2;
+ Z = 0.0 ;
+ } else {
+ X = KDL::atan2(-R(1,2), R(2,2));
+ Y = KDL::atan2(R(0,2), sqrt( KDL::sqr(R(0,0)) + KDL::sqr(R(0,1))));
+ Z = KDL::atan2(-R(0,1), R(0,0));
+ }
+}
+
static bool target_callback(const iTaSC::Timestamp& timestamp, const iTaSC::Frame& current, iTaSC::Frame& next, void *param)
{
IK_Target* target = (IK_Target*)param;
@@ -595,19 +608,19 @@
break;
case IK_XDOF|IK_YDOF|IK_ZDOF:
// RX+RZ+RY
- GetEulerXZY(bonerot, X, Z, Y);
+ GetEulerXYZ(bonerot, X, Y, Z);
joint += ":RX";
ret = arm->addSegment(joint, parent, KDL::Joint::RotX, X);
if (ret) {
parent = joint;
joint = bone->name;
- joint += ":RZ";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, Z);
+ joint += ":RY";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotY, Y);
if (ret) {
parent = joint;
joint = bone->name;
- joint += ":RY";
- ret = arm->addSegment(joint, parent, KDL::Joint::RotY, Y, tip);
+ joint += ":RZ";
+ ret = arm->addSegment(joint, parent, KDL::Joint::RotZ, Z, tip);
}
}
break;
@@ -802,7 +815,7 @@
if (ikscene->cache) {
iTaSC::CacheTS sts, cts, dts;
sts = cts = (iTaSC::CacheTS)(timestamp*1000.0);
- if (ikscene->cache->getPreviousCacheItem(NULL, 0, &cts) == NULL || cts == 0) {
+ if (ikscene->cache->getPreviousCacheItem(ikscene->armature, 0, &cts) == NULL || cts == 0) {
// the cache is empty before this time, reiterate
reiterate = true;
} else {
More information about the Bf-blender-cvs
mailing list