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 { _______________________________________________ Bf-blender-cvs mailing list Bf-blender-cvs@blender.org http://lists.blender.org/mailman/listinfo/bf-blender-cvs