/**
* @Inherit
*/
inline virtual void draw(SimViewer::SimViewer& viewer,
const Vector2D& posRoot, scalar angleRoot,
const Vector2D& posLeaf, scalar angleLeaf,
scalar value)
{
// Joint::draw(viewer, posRoot, angleRoot,
// posLeaf, angleLeaf, value);
Vector2D pos_j = posRoot + Vector2D::rotate(
Joint::getPosRoot(), angleRoot);
viewer.drawJoint(pos_j.x(), pos_j.y(),
(Joint::getAngleRoot()+angleRoot)*180.0/M_PI,
(value)*180.0/M_PI);
Vector2D pos = posLeaf + Vector2D::rotate(
Joint::getPosLeaf(), angleLeaf);
viewer.drawSegmentByEnd(posRoot.x(),posRoot.y(), pos_j.x(),pos_j.y() ,0.05,sf::Color(0,200,0,100));
// viewer.drawJoint(pos.x(), pos.y(),
// (Joint::getAngleRoot()+angleRoot)*180.0/M_PI,
// (value)*180.0/M_PI);
double ori=(Joint::getAngleLeaf()+angleLeaf);
Vector2D rot;
//Draw the cam
Vector2D pos_r=pos;
Vector2D pos_l=pos;
double x=0.0;
double y=0.0;
Leph::Symbolic::Bounder bounder;
SymbolPtr xs = Symbol::create("x");
for(int i=0;i<100;i++)
{
x+=.3/100.0;
xs->reset();
bounder.setValue(xs,x);
// y=F(x);
y=_F(xs)->evaluate(bounder);
rot=Vector2D::rotate(Vector2D(x,y),ori+M_PI);
viewer.drawSegmentByEnd(pos_r.x(),pos_r.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_r=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
xs->reset();
bounder.setValue(xs,-x);
y=_F(xs)->evaluate(bounder);
// y=F(-x);
rot=Vector2D::rotate(Vector2D(-x,y),ori+M_PI);
viewer.drawSegmentByEnd(pos_l.x(),pos_l.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_l=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
}
viewer.drawSegment(pos.x(),pos.y(),_sH*2.0,(Joint::getAngleLeaf()+angleLeaf-M_PI/2.0)*180.0/M_PI,0.05,sf::Color(255,255,255,100));
viewer.drawSegment(pos_j.x(),pos_j.y(),_sH,((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi)*180.0/M_PI,0.01,sf::Color(255,0,255,100));
viewer.drawCircle(pos_j.x()+_sH*cos((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi),pos_j.y()+_sH*sin((Joint::getAngleRoot()+angleRoot)+M_PI/2.0-_sphi),0.03,sf::Color(255,0,255,100));
viewer.drawCircle(pos.x(),pos.y(),0.03,sf::Color(255,255,255,255));
//Draw spring
Vector2D pos1 = posRoot + Vector2D::rotate(
Joint::getPosRoot(), angleRoot);
Vector2D pos2 = posLeaf + Vector2D::rotate(
Joint::getPosLeaf(), angleLeaf);
double zval=Vector2D::dist(pos1,pos2);
scalar tmp=zval/10.0;
scalar sig=1.0;
Vector2D tmppos=pos1;
scalar angle=atan2(pos2.y()-pos1.y(),pos2.x()-pos1.x());
Vector2D tmpnew=tmppos+Vector2D::rotate(Vector2D(0.1*sig,tmp), angle+M_PI/2.0+M_PI);
viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100));
tmppos=tmpnew;
sig*=-1.0;
tmp=zval/5.0;
for(int i=1;i<5;i++)
{
tmpnew=tmppos+Vector2D::rotate(Vector2D(0.2*sig,tmp), angle+M_PI/2.0+M_PI);
viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100));
tmppos=tmpnew;
sig*=-1.0;
}
tmp=zval/10.0;
tmpnew=tmppos+Vector2D::rotate(Vector2D(0.1*sig,tmp), angle+M_PI/2.0+M_PI);
viewer.drawSegmentByEnd(tmppos.x(),tmppos.y(), tmpnew.x(),tmpnew.y(),0.01,sf::Color(200,200,200,100));
}
/**
* @Inherit
*/
inline virtual void draw(SimViewer::SimViewer& viewer,
const Vector2D& posRoot, scalar angleRoot,
const Vector2D& posLeaf, scalar angleLeaf,
scalar value)
{
Joint::draw(viewer, posRoot, angleRoot,
posLeaf, angleLeaf, value);
// Vector2D pos = posRoot + Vector2D::rotate(
// Joint::getPosRoot(), angleRoot);
Vector2D pos = posLeaf + Vector2D::rotate(
Joint::getPosLeaf(), angleLeaf);
viewer.drawJoint(pos.x(), pos.y(),
(Joint::getAngleRoot()+angleRoot)*180.0/M_PI,
(value)*180.0/M_PI);
pos = posRoot + Vector2D::rotate(
Joint::getPosRoot(), angleRoot);
double ori=(Joint::getAngleRoot()+angleRoot);
Vector2D rot;
//Draw the cam
Vector2D pos_r=pos;
Vector2D pos_l=pos;
double x=0.0;
double y=0.0;
Leph::Symbolic::Bounder bounder;
SymbolPtr xs = Symbol::create("x");
for(int i=0;i<100;i++)
{
x+=.3/100.0;
xs->reset();
bounder.setValue(xs,x);
// y=F(x);
y=_F(xs)->evaluate(bounder);
// std::cout<<"x: "<<x<<" "<<xs->evaluate(bounder)<<" y: "<<y<<std::endl;
rot=Vector2D::rotate(Vector2D(x,y),ori);
viewer.drawSegmentByEnd(pos_r.x(),pos_r.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_r=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
xs->reset();
bounder.setValue(xs,-x);
y=_F(xs)->evaluate(bounder);
// y=F(-x);
rot=Vector2D::rotate(Vector2D(-x,y),ori);
viewer.drawSegmentByEnd(pos_l.x(),pos_l.y(), pos.x()+rot.x(),pos.y()+rot.y() ,0.01,sf::Color(200,200,200,100));
pos_l=Vector2D(pos.x()+rot.x(), pos.y()+rot.y());
}
viewer.drawSegment(pos.x(),pos.y(),_sH*2.0,((Joint::getAngleRoot()+angleRoot)+M_PI/2.0)*180.0/M_PI,0.05,sf::Color(255,255,255,100));
//Draw the lever
viewer.drawSegment(posLeaf.x(),posLeaf.y(),_sH,(angleLeaf-M_PI/2.0)*180.0/M_PI,0.01,sf::Color(255,0,255,100));
viewer.drawCircle(posLeaf.x()+_sH*cos(angleLeaf-M_PI/2.0),posLeaf.y()+_sH*sin(angleLeaf-M_PI/2.0),0.03,sf::Color(255,0,255,100));
viewer.drawCircle(pos.x(),pos.y(),0.03,sf::Color(255,255,255,255));
}
