/** * @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)); }