/*! Updates the geometry of the connectors between insertion points, which
need to move together with the robot's links. However, some of them
depend on two links, so we can not use link transforms directly (like
we do for the geometry of the insertion points).Rather, this geometry
needs to be recomputed after every change in link status.
*/
void Tendon::updateGeometry()
{
SbVec3f pPrev,pCur;
position tmpPos;
vec3 dPrev,dRes,c;
float m;
SoTransform *tran;
SoCylinder *geom;
SoTransform *neg90x = new SoTransform();
neg90x->rotation.setValue(SbVec3f(1.0f,0.0f,0.0f),-1.5707f);
/*first we wrap tendon around wrappers*/
checkWrapperIntersections();
/*and we remove unnecessary ones*/
removeWrapperIntersections();
/* we also compute the length of the tendon as the sum of connector lengths*/
currentLength = 0;
std::list<TendonInsertionPoint*>::iterator insPt, prevInsPt, newInsPt;
for (insPt=insPointList.begin(); insPt!=insPointList.end(); insPt++)
{
prevInsPt = insPt;
if (insPt!=insPointList.begin())
{
prevInsPt--;
pPrev = (*prevInsPt)->getWorldPosition();
pCur = (*insPt)->getWorldPosition();
dPrev = vec3(pCur) - vec3(pPrev);
/* distance between the two */
m = dPrev.len();
/* add it to tendon length */
currentLength += m;
/* midpoint between the two */
c = vec3(pPrev) + dPrev*0.5;
tran = (*insPt)->getIVConnectorTran();
geom = (*insPt)->getIVConnectorGeom();
geom->radius=(float)0.8;
geom->height = m;
tran->pointAt(c.toSbVec3f(),pPrev);
tran->combineLeft(neg90x);
}
}
/*after geometry has been updated, go ahead and update forces*/
computeSimplePassiveForces();
updateInsertionForces();
}
