[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [20874] branches/ge_dev: iTaSC: joint limit taken into account in automatic timestep adjustement.

[Benoit Bolsee]

Revision: 20874
          http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=20874
Author:   ben2610
Date:     2009-06-14 15:50:34 +0200 (Sun, 14 Jun 2009)

Log Message:
-----------
iTaSC: joint limit taken into account in automatic timestep adjustement.

Modified Paths:
--------------
    branches/ge_dev/intern/itasc/Armature.cpp
    branches/ge_dev/intern/itasc/Scene.cpp
    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-14 12:53:47 UTC (rev 20873)
+++ branches/ge_dev/intern/itasc/Armature.cpp	2009-06-14 13:50:34 UTC (rev 20874)
@@ -496,8 +496,45 @@
 	// then search through the joint limit
 	for (JointConstraintList::const_iterator it=m_constraints.begin(); it != m_constraints.end(); it++) {
 		JointConstraint_struct* cs = *it;
-
-		
+		if (cs->function == JointLimitCallback) {
+			// this is a joint limit constraint, 
+			LimitConstraintParam_struct* pLimit = (LimitConstraintParam_struct*)cs->param;
+			double qdot = m_qdot(cs->segment->second.q_nr);
+			if (fabs(qdot) > KDL::epsilon) {
+				double q = m_qKdl(cs->segment->second.q_nr);
+				double dq = qdot*timestep;
+				double newq = q+dq;
+				double alpha = cs->values.alpha;
+				double newalpha;
+				if (q    > pLimit->maxThreshold || q    < pLimit->minThreshold ||
+					newq > pLimit->maxThreshold || newq < pLimit->minThreshold) {
+					if (q > pLimit->maxThreshold) {
+						if (q < pLimit->max || dq < 0.0) {
+							newalpha = (alpha>2.0)?(alpha*((dq>0.0)?1.5:0.666)):(alpha+((dq>0.0)?1.0:-1.0));
+							if (newalpha > 0.0)
+								newq = pLimit->max-pLimit->threshold*(pLimit->maxWeight-newalpha)/(newalpha*pLimit->slope+pLimit->maxWeight);
+						}
+					} else if (q < pLimit->minThreshold) {
+						if (q > pLimit->min || dq > 0.0) {
+							newalpha = (alpha>2.0)?(alpha*((dq<0.0)?1.5:0.666)):(alpha+((dq<0.0)?1.0:-1.0));
+							if (newalpha > 0.0)
+								newq = pLimit->min+pLimit->threshold*(pLimit->maxWeight-newalpha)/(newalpha*pLimit->slope+pLimit->maxWeight);
+						}
+					} else if (newq > pLimit->maxThreshold) {
+						newalpha = 1.0;
+						newq = pLimit->max-pLimit->threshold*(pLimit->maxWeight-newalpha)/(newalpha*pLimit->slope+pLimit->maxWeight);
+					} else {
+						newalpha = 1.0;
+						newq = pLimit->min+pLimit->threshold*(pLimit->maxWeight-newalpha)/(newalpha*pLimit->slope+pLimit->maxWeight);
+					}
+					double newdq = fabs(newq-q);
+					dq = fabs(dq);
+					if (newdq < dq) {
+						timestep *= newdq/dq;
+					}
+				}
+			}
+		}
 	}
 	return timestep;
 }

Modified: branches/ge_dev/intern/itasc/Scene.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Scene.cpp	2009-06-14 12:53:47 UTC (rev 20873)
+++ branches/ge_dev/intern/itasc/Scene.cpp	2009-06-14 13:50:34 UTC (rev 20874)
@@ -397,7 +397,7 @@
 				ConstraintSet_struct* cs = it->second;
 				cs->task->getMaxTimestep(timesubstep);
 			}
-			// use substep that are dividers of timestep for more regularity
+			// use substep that are even dividers of timestep for more regularity
 			maxsubstep = 2.0*floor(timestep/2.0/timesubstep-0.66666);
 			timesubstep = (maxsubstep < 0.0) ? timestep : timestep/(2.0+maxsubstep);
 			if (timesubstep >= timeleft-(m_minstep/2.0)) {

Modified: branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp	2009-06-14 12:53:47 UTC (rev 20873)
+++ branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp	2009-06-14 13:50:34 UTC (rev 20874)
@@ -692,19 +692,19 @@
 		if ((flag & IK_XDOF) && (pchan->ikflag & BONE_IK_XLIMIT)) {
 			joint = bone->name;
 			joint += ":RX";
-			if (arm->addLimitConstraint(joint, KDL::deg2rad*pchan->limitmin[0], KDL::deg2rad*pchan->limitmax[0], KDL::deg2rad*5.0, 10.0, 2.0) < 0)
+			if (arm->addLimitConstraint(joint, pchan->limitmin[0], pchan->limitmax[0], 5.0, 50.0, 5.0) < 0)
 				break;
 		}
 		if ((flag & IK_YDOF) && (pchan->ikflag & BONE_IK_YLIMIT)) {
 			joint = bone->name;
 			joint += ":RY";
-			if (arm->addLimitConstraint(joint, KDL::deg2rad*pchan->limitmin[1], KDL::deg2rad*pchan->limitmax[1], KDL::deg2rad*5.0, 10.0, 2.0) < 0)
+			if (arm->addLimitConstraint(joint, pchan->limitmin[1], pchan->limitmax[1], 5.0, 50.0, 5.0) < 0)
 				break;
 		}
 		if ((flag & IK_ZDOF) && (pchan->ikflag & BONE_IK_ZLIMIT)) {
 			joint = bone->name;
 			joint += ":RZ";
-			if (arm->addLimitConstraint(joint, KDL::deg2rad*pchan->limitmin[2], KDL::deg2rad*pchan->limitmax[2], KDL::deg2rad*5.0, 10.0, 2.0) < 0)
+			if (arm->addLimitConstraint(joint, pchan->limitmin[2], pchan->limitmax[2], 5.0, 50.0, 5.0) < 0)
 				break;
 		}
 		//  no error, so restore