Revision: 21152
http://projects.blender.org/plugins/scmsvn/viewcvs.php?view=rev&root=bf-blender&revision=21152
Author: ben2610
Date: 2009-06-25 13:57:19 +0200 (Thu, 25 Jun 2009)
Log Message:
-----------
iTaSC: implement 2DoF Swing joint, connect iTaSC cache reset to BKE point
cache. Fix bug in KDL on Vector2 Norm. Add Helper functions GetValue to get
vector coordinates from KDL.
Modified Paths:
--------------
branches/ge_dev/intern/itasc/Armature.cpp
branches/ge_dev/intern/itasc/Cache.cpp
branches/ge_dev/intern/itasc/Cache.hpp
branches/ge_dev/intern/itasc/kdl/frames.cpp
branches/ge_dev/intern/itasc/kdl/frames.hpp
branches/ge_dev/intern/itasc/kdl/frames.inl
branches/ge_dev/intern/itasc/kdl/joint.cpp
branches/ge_dev/intern/itasc/kdl/joint.hpp
branches/ge_dev/intern/itasc/kdl/segment.cpp
branches/ge_dev/intern/itasc/kdl/segment.hpp
branches/ge_dev/intern/itasc/kdl/treejnttojacsolver.cpp
branches/ge_dev/source/blender/blenkernel/intern/pointcache.c
branches/ge_dev/source/blender/ikplugin/BIK_api.h
branches/ge_dev/source/blender/ikplugin/intern/ikplugin_api.c
branches/ge_dev/source/blender/ikplugin/intern/ikplugin_api.h
branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.cpp
branches/ge_dev/source/blender/ikplugin/intern/itasc_plugin.h
Modified: branches/ge_dev/intern/itasc/Armature.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Armature.cpp 2009-06-25 10:52:09 UTC (rev
21151)
+++ branches/ge_dev/intern/itasc/Armature.cpp 2009-06-25 11:57:19 UTC (rev
21152)
@@ -364,11 +364,18 @@
for (unsigned int q_nr=0; q_nr<m_nq; ) {
Joint_struct& joint = m_joints[q_nr];
switch (joint.type) {
+ case KDL::Joint::Swing:
+ {
+ double* qdot=&m_qdotKdl(q_nr);
+ double* q=&m_qKdl(q_nr);
+
(KDL::Rot(KDL::Vector(q[0],0.0,q[1]))*KDL::Rot(KDL::Vector(qdot[0],0.0,qdot[1])*timestamp.realTimestep)).GetXZRot().GetValue(q);
+ break;
+ }
case KDL::Joint::Sphere:
{
double* qdot=&m_qdotKdl(q_nr);
double* q=&m_qKdl(q_nr);
-
(KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)*KDL::Rot(KDL::Vector(q))).GetRot().GetValue(q);
+
(KDL::Rot(KDL::Vector(q))*KDL::Rot(KDL::Vector(qdot)*timestamp.realTimestep)).GetRot().GetValue(q);
break;
}
default:
Modified: branches/ge_dev/intern/itasc/Cache.cpp
===================================================================
--- branches/ge_dev/intern/itasc/Cache.cpp 2009-06-25 10:52:09 UTC (rev
21151)
+++ branches/ge_dev/intern/itasc/Cache.cpp 2009-06-25 11:57:19 UTC (rev
21152)
@@ -305,6 +305,83 @@
return 0;
}
+void Cache::clearCacheFrom(const void *device, CacheTS timestamp)
+{
+ CacheMap::iterator it = (device) ? m_cache.find(device) :
m_cache.begin();
+ CacheEntry *entry;
+ CacheChannel *channel;
+ CacheBuffer *buffer, *nextBuffer, *prevBuffer;
+ CacheItem *item, *prevItem, *nextItem;
+ unsigned int positionW, block;
+
+ while (it != m_cache.end()) {
+ entry = it->second;
+ for (unsigned int ch=0; ch<entry->m_count; ch++) {
+ channel = &entry->m_channelArray[ch];
+ if (channel->m_busy) {
+ item = channel->findItemOrLater(timestamp,
&buffer);
+ if (item ) {
+ // this item and all later items will
be removed, clear any later buffer
+ while ((nextBuffer = buffer->m_next) !=
NULL) {
+ buffer->m_next =
nextBuffer->m_next;
+ free(nextBuffer);
+ }
+ positionW =
CACHE_ITEM_POSITIONW(buffer,item);
+ if (positionW == 0) {
+ // this item is the first one
of the buffer, remove the buffer completely
+ // first find the buffer just
before it
+ nextBuffer =
channel->m_firstBuffer;
+ prevBuffer = NULL;
+ while (nextBuffer != buffer) {
+ prevBuffer = nextBuffer;
+ nextBuffer =
nextBuffer->m_next;
+ // we must quit this
loop before reaching the end of the list
+ assert(nextBuffer);
+ }
+ free(buffer);
+ buffer = prevBuffer;
+ if (buffer == NULL)
+ // this was also the
first buffer
+ channel->m_firstBuffer
= NULL;
+ } else {
+ // removing this item means
finding the previous item to make it the last one
+ block =
positionW>>channel->m_positionToBlockShiftW;
+ if (block == 0) {
+ // start from first
item, we know it is not our item because positionW > 0
+ prevItem =
&buffer->m_firstItem;
+ } else {
+ // no need to check the
current block, it will point to our item or a later one
+ // but the previous
block will be a good start for sure.
+ block--;
+ prevItem =
CACHE_BLOCK_ITEM_ADDR(channel,buffer,block);
+ }
+ while ((nextItem =
CACHE_NEXT_ITEM(prevItem)) < item)
+ prevItem = nextItem;
+ // we must have found our item
+ assert(nextItem==item);
+ // now set the buffer
+ buffer->m_lastItemPositionW =
CACHE_ITEM_POSITIONW(buffer,prevItem);
+ buffer->m_firstFreePositionW =
positionW;
+ buffer->m_lastTimestamp =
buffer->m_firstTimestamp + prevItem->m_timeOffset;
+ block =
buffer->m_lastItemPositionW>>channel->m_positionToBlockShiftW;
+ buffer->lookup[block].m_offsetW
= buffer->m_lastItemPositionW&channel->m_positionToOffsetMaskW;
+
buffer->lookup[block].m_timeOffset = prevItem->m_timeOffset;
+ }
+ // set the channel
+ channel->m_lastBuffer = buffer;
+ if (buffer) {
+ channel->m_lastTimestamp =
buffer->m_lastTimestamp;
+ channel->m_lastItemPositionW =
buffer->m_lastItemPositionW;
+ }
+ }
+ }
+ }
+ if (device)
+ break;
+ ++it;
+ }
+}
+
void *Cache::addCacheItem(const void *device, int id, unsigned int timestamp,
void *data, unsigned int length)
{
CacheMap::iterator it = m_cache.find(device);
Modified: branches/ge_dev/intern/itasc/Cache.hpp
===================================================================
--- branches/ge_dev/intern/itasc/Cache.hpp 2009-06-25 10:52:09 UTC (rev
21151)
+++ branches/ge_dev/intern/itasc/Cache.hpp 2009-06-25 11:57:19 UTC (rev
21152)
@@ -101,6 +101,9 @@
int deleteChannel(const void *device, int channel);
/* delete all channels of a device and remove the device from the map */
int deleteDevice(const void *device);
+ /* removes all cache items, leaving the special item at timestamp=0.
+ if device=NULL, apply to all devices. */
+ void clearCacheFrom(const void *device, CacheTS timestamp);
/* add a new cache item
channel: the cache channel (as returned by AddChannel
Modified: branches/ge_dev/intern/itasc/kdl/frames.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.cpp 2009-06-25 10:52:09 UTC (rev
21151)
+++ branches/ge_dev/intern/itasc/kdl/frames.cpp 2009-06-25 11:57:19 UTC (rev
21152)
@@ -83,10 +83,14 @@
double Vector2::Norm() const
{
- if (fabs(data[0]) > fabs(data[1]) ) {
- return data[0]*sqrt(1+sqr(data[1]/data[0]));
+ double tmp0 = fabs(data[0]);
+ double tmp1 = fabs(data[1]);
+ if (tmp0 >= tmp1) {
+ if (tmp1 == 0)
+ return 0;
+ return tmp0*sqrt(1+sqr(tmp1/tmp0));
} else {
- return data[1]*sqrt(1+sqr(data[0]/data[1]));
+ return tmp1*sqrt(1+sqr(tmp0/tmp1));
}
}
// makes v a unitvector and returns the norm of v.
@@ -320,6 +324,18 @@
return axis * alfa;
}
+Vector2 Rotation::GetXZRot() const
+{
+ // [0,1,0] x Y
+ Vector2 axis(data[7], -data[1]);
+ double norm = axis.Normalize();
+ if (norm < epsilon) {
+ norm = (data[4] < 0.0) ? PI : 0.0;
+ } else {
+ norm = acos(data[4]);
+ }
+ return axis*norm;
+}
/** Returns the rotation angle around the equiv. axis
Modified: branches/ge_dev/intern/itasc/kdl/frames.hpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.hpp 2009-06-25 10:52:09 UTC (rev
21151)
+++ branches/ge_dev/intern/itasc/kdl/frames.hpp 2009-06-25 11:57:19 UTC (rev
21152)
@@ -370,6 +370,10 @@
//! and its norm is angle
Vector GetRot() const;
+ //! Returns a 2D vector representing the equivalent rotation in the XZ
plane that brings the
+ //! Y axis onto the Matrix Y axis and its norm is angle
+ Vector2 GetXZRot() const;
+
/** Returns the rotation angle around the equiv. axis
* @param axis the rotation axis is returned in this variable
* @param eps : in the case of angle == 0 : rot axis is undefined and
choosen
@@ -917,6 +921,9 @@
//! Access to elements, range checked when NDEBUG is not set, from 0..1
inline double& operator() (int index);
+ //! store vector components in array
+ inline void GetValue(double* xy);
+
inline void ReverseSign();
inline Vector2& operator-=(const Vector2& arg);
inline Vector2& operator +=(const Vector2& arg);
Modified: branches/ge_dev/intern/itasc/kdl/frames.inl
===================================================================
--- branches/ge_dev/intern/itasc/kdl/frames.inl 2009-06-25 10:52:09 UTC (rev
21151)
+++ branches/ge_dev/intern/itasc/kdl/frames.inl 2009-06-25 11:57:19 UTC (rev
21152)
@@ -757,6 +757,10 @@
return *this;
}
+IMETHOD void Vector2::GetValue(double* xy)
+{
+ xy[0]=data[0];xy[1]=data[1];
+}
IMETHOD Vector2 operator +(const Vector2 & lhs,const Vector2& rhs)
{
Modified: branches/ge_dev/intern/itasc/kdl/joint.cpp
===================================================================
--- branches/ge_dev/intern/itasc/kdl/joint.cpp 2009-06-25 10:52:09 UTC (rev
21151)
+++ branches/ge_dev/intern/itasc/kdl/joint.cpp 2009-06-25 11:57:19 UTC (rev
21152)
@@ -27,21 +27,11 @@
const double& _inertia, const double& _damping, const double&
_stiffness):
type(_type),scale(_scale),offset(_offset),inertia(_inertia),damping(_damping),stiffness(_stiffness)
{
- // this constructor should not be used for sphere joint, assume
no offset in basis
+ // for sphere and swing, offset is not used, assume no offset
}
- Joint::Joint(const JointType& _type, const double& _scale, const Rotation&
_basis,
- const double& _inertia, const double& _damping, const double&
_stiffness):
-
type(_type),scale(_scale),offset(0.0),inertia(_inertia),damping(_damping),stiffness(_stiffness)
- {
- // this constructor should not be used for 1DOF joint
- if (_type == Sphere) {
- basis = _basis;
- }
- }
-
Joint::Joint(const Joint& in):
- type(in.type),scale(in.scale),offset(in.offset),basis(in.basis),
+ type(in.type),scale(in.scale),offset(in.offset),
inertia(in.inertia),damping(in.damping),stiffness(in.stiffness)
{
}
@@ -51,7 +41,6 @@
type=in.type;
scale=in.scale;
offset=in.offset;
- basis=in.basis;
inertia=in.inertia;
damping=in.damping;
stiffness=in.stiffness;
@@ -88,8 +77,13 @@
case Sphere:
// the joint angles represent a rotation vector
expressed in the base frame of the joint
// (= the frame you get when there is no offset nor
rotation)
- return Frame(Rot(Vector((&q)[0], (&q)[1],
(&q)[2]))*basis);
+ return Frame(Rot(Vector((&q)[0], (&q)[1], (&q)[2])));
break;
+ case Swing:
+ // the joint angles represent a 2D rotation vector in
the XZ planee of the base frame of the joint
+ // (= the frame you get when there is no offset nor
rotation)
+ return Frame(Rot(Vector((&q)[0], 0.0, (&q)[1])));
+ break;
default:
return Frame::Identity();
break;
@@ -117,6 +111,14 @@
case TransZ:
return Twist(Vector(0.0,0.0,scale*qdot),Vector(0.0,0.0,0.0));
break;
+ case Swing:
+ switch (dof) {
+ case 0:
+ return
Twist(Vector(0.0,0.0,0.0),Vector(scale*qdot,0.0,0.0));
+ case 1:
+ return
Twist(Vector(0.0,0.0,0.0),Vector(0.0,0.0,scale*qdot));
+ }
+ return Twist::Zero();
case Sphere:
@@ Diff output truncated at 10240 characters. @@
_______________________________________________
Bf-blender-cvs mailing list
[email protected]
http://lists.blender.org/mailman/listinfo/bf-blender-cvs
