void LinkedStructure::update()
{
// Performs an update to the position of the
// linked structure. Performs a tip move by the
// step that has been specified
if(isTargetResolved()) return;
float x = (mTargetPosition(0) - mPosition(0) > 1) ? mStep : -mStep;
float y = (mTargetPosition(1) + mPosition(1) > 1) ? mStep : -mStep;
moveBy(x, y);
}
bool LinkedStructure::isTargetResolved()
{
// Checks if at the current position of the linked
// structure the point we are trying to resolve
// gets resolved
if(fabs(mPosition(0) - mTargetPosition(0)) <= 2.0f &&
fabs(mPosition(1) + mTargetPosition(1)) <= 2.0f)
{
mResolveTarget = false;
return true;
}
return false;
}
void LinkedStructure::calculatePosition()
{
float angle = 0;
mPosition = VectorXf::Zero(2,1);
// Computing the new position in a similar way
// that we construct the jacobian. Can be seen
// also as some type of series
for (int i = 0; i < mList.size(); i++)
{
angle += mList[i]->mAngle;
mPosition(0) += mList[i]->mLength*cos(angle);
mPosition(1) += -mList[i]->mLength*sin(angle);
}
}
inline Position operator -(const Position p, const vec2 v) {
return mPosition(p.x - PositionFixed(v.x), p.y - PositionFixed(v.y));
}