[Bf-blender-cvs] SVN commit: /data/svn/bf-blender [22052] branches/itasc: iTaSC: support polar constraint.
Benoit Bolsee
benoit.bolsee at online.be
Thu Jul 30 16:41:52 CEST 2009
Revision: 22052
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=22052
Author: ben2610
Date: 2009-07-30 16:41:52 +0200 (Thu, 30 Jul 2009)
Log Message:
-----------
iTaSC: support polar constraint. Polar constraint is handled as an additional rotation of the armature base. This is equivalent to the math of the old solver and it produces the same result. Add necessary support in KDL. Fix loose pointer in some actuators: add cleanup code when object is deleted.
Modified Paths:
--------------
branches/itasc/intern/itasc/Cache.hpp
branches/itasc/intern/itasc/Scene.cpp
branches/itasc/intern/itasc/kdl/frames.hpp
branches/itasc/intern/itasc/kdl/frames.inl
branches/itasc/source/blender/blenkernel/intern/sca.c
branches/itasc/source/blender/blenloader/intern/readfile.c
branches/itasc/source/blender/editors/space_logic/logic_window.c
branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
branches/itasc/source/blender/makesdna/DNA_actuator_types.h
branches/itasc/source/gameengine/Converter/BL_ArmatureActuator.cpp
branches/itasc/source/gameengine/Converter/BL_ArmatureActuator.h
branches/itasc/source/gameengine/Converter/KX_ConvertActuators.cpp
Modified: branches/itasc/intern/itasc/Cache.hpp
===================================================================
--- branches/itasc/intern/itasc/Cache.hpp 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/intern/itasc/Cache.hpp 2009-07-30 14:41:52 UTC (rev 22052)
@@ -39,7 +39,8 @@
unsigned int substep:1;
unsigned int reiterate:1;
unsigned int cache:1;
- unsigned int dummy:29;
+ unsigned int update:1;
+ unsigned int dummy:28;
Timestamp() { memset(this, 0, sizeof(Timestamp)); }
};
Modified: branches/itasc/intern/itasc/Scene.cpp
===================================================================
--- branches/itasc/intern/itasc/Scene.cpp 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/intern/itasc/Scene.cpp 2009-07-30 14:41:52 UTC (rev 22052)
@@ -240,6 +240,7 @@
// reiteration=additional iteration with same timestamp if application finds the convergence not good enough
ts.reiterate = (reiterate) ? 1 : 0;
ts.cache = (cache) ? 1 : 0;
+ ts.update = 1;
bool autosubstep = (numsubstep == 0) ? true : false;
if (numsubstep < 1)
numsubstep = 1;
Modified: branches/itasc/intern/itasc/kdl/frames.hpp
===================================================================
--- branches/itasc/intern/itasc/kdl/frames.hpp 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/intern/itasc/kdl/frames.hpp 2009-07-30 14:41:52 UTC (rev 22052)
@@ -153,13 +153,16 @@
//! Constructs a vector out of an array of three values x, y and z
inline Vector(double* xyz);
+ //! Constructs a vector out of an array of three values x, y and z
+ inline Vector(float* xyz);
+
//! Assignment operator. The normal copy by value semantics.
inline Vector(const Vector& arg);
//! store vector components in array
- inline void GetValue(double* xyz);
+ inline void GetValue(double* xyz) const;
- //! Assignment operator. The normal copy by value semantics.
+ //! Assignment operator. The normal copy by value semantics.
inline Vector& operator = ( const Vector& arg);
//! Access to elements, range checked when NDEBUG is not set, from 0..2
@@ -244,6 +247,9 @@
//! different. It compares whether the 2 arguments are equal in an eps-interval
inline friend bool Equal(const Vector& a,const Vector& b,double eps=epsilon);
+ //! return a normalized vector
+ inline friend Vector Normalize(const Vector& a, double eps=epsilon);
+
//! The literal equality operator==(), also identical.
inline friend bool operator==(const Vector& a,const Vector& b);
//! The literal inequality operator!=().
@@ -302,7 +308,7 @@
// default copy constructor is sufficient
inline void setValue(float* oglmat);
- inline void getValue(float* oglmat);
+ inline void getValue(float* oglmat) const;
inline Rotation& operator=(const Rotation& arg);
@@ -530,7 +536,7 @@
explicit inline Frame(const Rotation& R);
inline void setValue(float* oglmat);
- inline void getValue(float* oglmat);
+ inline void getValue(float* oglmat) const;
inline Frame() {}
//! The copy constructor. Normal copy by value semantics.
@@ -922,7 +928,7 @@
inline double& operator() (int index);
//! store vector components in array
- inline void GetValue(double* xy);
+ inline void GetValue(double* xy) const;
inline void ReverseSign();
inline Vector2& operator-=(const Vector2& arg);
Modified: branches/itasc/intern/itasc/kdl/frames.inl
===================================================================
--- branches/itasc/intern/itasc/kdl/frames.inl 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/intern/itasc/kdl/frames.inl 2009-07-30 14:41:52 UTC (rev 22052)
@@ -43,8 +43,13 @@
data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2];
}
-IMETHOD void Vector::GetValue(double* xyz)
+IMETHOD Vector::Vector(float* xyz)
{
+ data[0]=xyz[0];data[1]=xyz[1];data[2]=xyz[2];
+}
+
+IMETHOD void Vector::GetValue(double* xyz) const
+{
xyz[0]=data[0];xyz[1]=data[1];xyz[2]=data[2];
}
@@ -154,8 +159,12 @@
return data[index];
}
+IMETHOD Vector Normalize(const Vector& a, double eps)
+{
+ double l=a.Norm();
+ return (l<eps) ? Vector(0.0,0.0,0.0) : a/l;
+}
-
Wrench Frame::operator * (const Wrench& arg) const
// Complexity : 24M+18A
{
@@ -644,7 +653,7 @@
Ortho();
}
-void Rotation::getValue(float* oglmat)
+void Rotation::getValue(float* oglmat) const
{
*oglmat++ = (float)data[0]; *oglmat++ = (float)data[3]; *oglmat++ = (float)data[6]; *oglmat++ = 0.f;
*oglmat++ = (float)data[1]; *oglmat++ = (float)data[4]; *oglmat++ = (float)data[7]; *oglmat++ = 0.f;
@@ -712,7 +721,7 @@
p.data[2] = oglmat[14];
}
-void Frame::getValue(float* oglmat)
+void Frame::getValue(float* oglmat) const
{
M.getValue(oglmat);
oglmat[12] = (float)p.data[0];
@@ -757,7 +766,7 @@
return *this;
}
-IMETHOD void Vector2::GetValue(double* xy)
+IMETHOD void Vector2::GetValue(double* xy) const
{
xy[0]=data[0];xy[1]=data[1];
}
Modified: branches/itasc/source/blender/blenkernel/intern/sca.c
===================================================================
--- branches/itasc/source/blender/blenkernel/intern/sca.c 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/source/blender/blenkernel/intern/sca.c 2009-07-30 14:41:52 UTC (rev 22052)
@@ -616,6 +616,8 @@
bEditObjectActuator *eoa;
bPropertyActuator *pa;
bMessageActuator *ma;
+ bParentActuator *para;
+ bArmatureActuator *aa;
sens= obt->sensors.first;
while(sens) {
@@ -654,7 +656,15 @@
ma= act->data;
if(ma->toObject==ob) ma->toObject= NULL;
break;
-
+ case ACT_PARENT:
+ para = act->data;
+ if (para->ob==ob) para->ob = NULL;
+ break;
+ case ACT_ARMATURE:
+ aa = act->data;
+ if (aa->target == ob) aa->target = NULL;
+ if (aa->subtarget == ob) aa->subtarget = NULL;
+ break;
}
act= act->next;
}
Modified: branches/itasc/source/blender/blenloader/intern/readfile.c
===================================================================
--- branches/itasc/source/blender/blenloader/intern/readfile.c 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/source/blender/blenloader/intern/readfile.c 2009-07-30 14:41:52 UTC (rev 22052)
@@ -3531,6 +3531,7 @@
else if(act->type==ACT_ARMATURE) {
bArmatureActuator *arma= act->data;
arma->target= newlibadr(fd, ob->id.lib, arma->target);
+ arma->subtarget= newlibadr(fd, ob->id.lib, arma->subtarget);
}
act= act->next;
}
Modified: branches/itasc/source/blender/editors/space_logic/logic_window.c
===================================================================
--- branches/itasc/source/blender/editors/space_logic/logic_window.c 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/source/blender/editors/space_logic/logic_window.c 2009-07-30 14:41:52 UTC (rev 22052)
@@ -2777,7 +2777,7 @@
case ACT_ARMATURE:
armAct = act->data;
- ysize= 48;
+ ysize= 68;
glRects(xco, yco-ysize, xco+width, yco);
uiEmboss((float)xco, (float)yco-ysize, (float)xco+width, (float)yco, 1);
if (ob->type == OB_ARMATURE) {
@@ -2794,6 +2794,7 @@
uiButSetFunc(but, check_armature_actuator, but, armAct);
uiBlockEndAlign(block);
uiDefIDPoinBut(block, test_obpoin_but, ID_OB, 1, "Target: ", xco+5, yco-44, (width-10), 19, &(armAct->target), "Set this object as the target of the constraint");
+ uiDefIDPoinBut(block, test_obpoin_but, ID_OB, 1, "Secondary Target: ", xco+5, yco-64, (width-10), 19, &(armAct->subtarget), "Set this object as the secondary target of the constraint (only IK polar target at the moment)");
}
yco-= ysize;
Modified: branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp
===================================================================
--- branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-07-30 13:56:39 UTC (rev 22051)
+++ branches/itasc/source/blender/ikplugin/intern/itasc_plugin.cpp 2009-07-30 14:41:52 UTC (rev 22052)
@@ -107,7 +107,8 @@
bPoseChannel* pchan; // channel where we must copy matrix back
int parent; // index in this array of the parent channel
KDL::Frame frame; // frame of the bone relative to object base, not armature base
- std::string bone; // segment name of the joint from which we get the bone tail and head
+ std::string tail; // segment name of the joint from which we get the bone tail
+ std::string head; // segment name of the joint from which we get the bone head
IK_Channel() {
pchan = NULL;
@@ -128,6 +129,8 @@
KDL::Frame baseFrame; // frame of armature base relative to blArmature
iTaSC::WSDLSSolver* solver;
Object* blArmature;
+ struct bConstraint* polarConstraint;
+
std::vector<IK_Target*> targets;
@@ -141,6 +144,7 @@
solver = NULL;
blArmature = NULL;
numchan = 0;
+ polarConstraint = NULL;
}
~IK_Scene() {
@@ -460,6 +464,70 @@
ikscene->baseFrame = iTaSC::F_identity;
}
next.setValue(&rootmat[0][0]);
+ // if there is a polar target (only during solving otherwise we don't have end efffector)
+ if (ikscene->polarConstraint && timestamp.update) {
+ // compute additional rotation of base frame so that armature follows the polar target
+ float imat[4][4]; // IK tree base inverse matrix
+ float polemat[4][4]; // polar target in IK tree base frame
+ float goalmat[4][4]; // target in IK tree base frame
+ float mat[4][4]; // temp matrix
+ bKinematicConstraint* poledata = (bKinematicConstraint*)ikscene->polarConstraint->data;
+
+ Mat4Invert(imat, rootmat);
+ // polar constraint imply only one target
+ IK_Target *iktarget = ikscene->targets[0];
+ // root channel from which we take the bone initial orientation
+ IK_Channel &rootchan = ikscene->channels[0];
+
+ // get polar target matrix in world space
+ get_constraint_target_matrix(ikscene->polarConstraint, 1, CONSTRAINT_OBTYPE_OBJECT, ikscene->blArmature, mat, 1.0);
+ // convert to armature space
+ Mat4MulMat4(polemat, mat, imat);
+ // get the target in world space (was computed before as target object are defined before base object)
+ iktarget->target->getPose().getValue(mat[0]);
+ // convert to armature space
+ Mat4MulMat4(goalmat, mat, imat);
+ // take position of target, polar target, end effector, in armature space
+ KDL::Vector goalpos(goalmat[3]);
+ KDL::Vector polepos(polemat[3]);
+ KDL::Vector endpos = ikscene->armature->getPose(iktarget->ee).p;
+ // get root bone orientation
+ KDL::Frame rootframe;
@@ Diff output truncated at 10240 characters. @@
More information about the Bf-blender-cvs
mailing list