/**
* @Inherit
*/
inline virtual void computeSymTransformation(SymbolPtr time)
{
//Get linked Bodies
Body& root = Joint::getBodyRoot();
Body& leaf = Joint::getBodyLeaf();
//Get Joint structure
const Vector2D& posRoot = Joint::getPosRoot();
const Vector2D& posLeaf = Joint::getPosLeaf();
scalar angleRoot = Joint::getAngleRoot();
scalar angleLeaf = Joint::getAngleLeaf();
//Build Joint structure Constants
TermPtr angleRootSym = Constant::create(angleRoot);
TermPtr angleLeafSym = Constant::create(angleLeaf);
TermVectorPtr posRootSym = ConstantVector::create(posRoot);
TermVectorPtr posLeafSym = ConstantVector::create(posLeaf);
//Build unity vector
TermVectorPtr unitySym = ConstantVector::
create(Vector2D(1.0, 0.0));
//Get Joint degree of freedom
SymbolPtr dof = Joint::getDof();
//Sum up angle
TermPtr angle1 = Symbolic::Add<scalar>::create(
root.getSymAngle(),
Symbolic::Add<scalar>::create(
dof,
angleRootSym));
//Build up Leaf Body orientation
TermPtr resultAngle = Symbolic::Add<scalar>::create(
angle1,
Constant::create(-angleLeaf));
//Joint anchor on Root Body and Leaf Body
TermVectorPtr p1 = Symbolic::Add<Vector2D>::create(
root.getSymPosition(),
Symbolic::Rotation<Vector2D, scalar>::create(
posRootSym,
root.getSymAngle()));
//Build up center of Leaf Body
TermVectorPtr resultPos = Symbolic::Add<Vector2D>::create(
p1,
Symbolic::Rotation<Vector2D, scalar>::create(
Symbolic::Minus<Vector2D>::create(posLeafSym),
resultAngle));
//Set up Leaf Symbols
leaf.initSymbols(
resultPos,
resultAngle,
resultPos->derivate(time),
resultAngle->derivate(time));
}
/**
* @Inherit
*/
inline virtual void computeSymTransformation(SymbolPtr time)
{
//Get linked Bodies
Body& root = Joint::getBodyRoot();
Body& leaf = Joint::getBodyLeaf();
//Get Joint structure
const Vector2D& posRoot = Joint::getPosRoot();
const Vector2D& posLeaf = Joint::getPosLeaf();
scalar angleRoot = Joint::getAngleRoot();
scalar angleLeaf = Joint::getAngleLeaf();
//Build Joint structure Constants
TermPtr angleRootSym = Constant::create(angleRoot);
TermPtr angleLeafSym = Constant::create(angleLeaf);
TermVectorPtr posRootSym = ConstantVector::create(posRoot);
TermVectorPtr posLeafSym = ConstantVector::create(posLeaf);
//Build unity vector
TermVectorPtr unitySym = ConstantVector::
create(Vector2D(0.0, 1.0));
//Get Joint degree of freedom
SymbolPtr dof = Joint::getDof();
//Sum up angle
TermPtr angle1 = Symbolic::Add<scalar>::create(
root.getSymAngle(),
Symbolic::Add<scalar>::create(
dof,
angleRootSym));
TermPtr angle_root = Symbolic::Add<scalar>::create(root.getSymAngle(),angleRootSym);
//Build up Leaf Body orientation
// TermPtr resultAngle = Symbolic::Add<scalar>::create(
// angle1,
// Symbolic::Add<scalar>::create(Constant::create(-angleLeaf),_phi)); //Offset
TermPtr resultAngle = Symbolic::Add<scalar>::create(
angle1,
Constant::create(-angleLeaf)); //Offset
//Joint anchor on Root Body and Leaf Body
TermVectorPtr p1 = Symbolic::Add<Vector2D>::create(
root.getSymPosition(),
Symbolic::Rotation<Vector2D, scalar>::create(
posRootSym,
root.getSymAngle()));
TermPtr z=Symbolic::Add<scalar>::create(_F(Symbolic::Mult<scalar, scalar, scalar>::create(Symbolic::Minus<scalar>::create(_H),SIN(Symbolic::Add<scalar>::create(dof,_phi)))),Symbolic::Mult<scalar, scalar, scalar>::create(_H,COS(Symbolic::Add<scalar>::create(dof,_phi))));
_sl=z;//keep track of the translation for the spring
//Joint anchor on Leaf Body
// TermVectorPtr p2 = Symbolic::Add<Vector2D>::create(
// p1,
// Symbolic::Mult<Vector2D, scalar, Vector2D>::create(
// z,
// Symbolic::Rotation<Vector2D, scalar>::create(
// unitySym,
// angle_root)));
TermVectorPtr p2 = Symbolic::Add<Vector2D>::create(
p1,
Symbolic::Mult<Vector2D, scalar, Vector2D>::create(
z,
Symbolic::Rotation<Vector2D, scalar>::create(
unitySym,
angle1)));
//Build up center of Leaf Body
TermVectorPtr resultPos = Symbolic::Add<Vector2D>::create(
p2,
Symbolic::Rotation<Vector2D, scalar>::create(
Symbolic::Minus<Vector2D>::create(posLeafSym),
resultAngle));
//Set up Leaf Symbols
leaf.initSymbols(
resultPos,
// Symbolic::Add<scalar>::create(resultAngle,_phi),
resultAngle,
resultPos->derivate(time),
resultAngle->derivate(time));
}
