/*-----------------------------------------------------------------------------
* Simulation
*-----------------------------------------------------------------------------*/
void onAnimate(){
/// Line Symmetric Case
R3=R0; R4=R1; R5=R2;
theta3=theta0; theta4=theta1; theta5=theta2;
float R[] = {R0,R1,R2,R3,R4,R5};
float theta[] = {theta0,theta1,theta2,theta3,theta4,theta5};
for (int i=0;i<6;++i){
auto rot = Gen::rot( Biv::xz * theta[i]);
chain.link(i).pos( Vec(0,3,0) + Vec(0,0,R[i]) );//.spin(rot) );
chain.link(i).rot() = rot;
}
chain.joint(0).rot( Biv::xy * amt0 );
chain.joint(1).rot( Biv::xy * amt1 );
chain.joint(2).rot( Biv::xy * amt2 );
chain.joint(3).rot( Biv::xy * amt3 );
chain.fk();
chain[5] = Frame( !chain.link(5).mot() * chain[0].mot() );
// auto s2 = chain.lastDls(5);
//chain.calcJoints();
}
/*-----------------------------------------------------------------------------
* Draw Routines
*-----------------------------------------------------------------------------*/
void onDraw(){
if (bMouseSet) mouse = calcMouse3D();
reset();
chain.joint(1).rot() = Gen::rot(Biv::xy * amt1);
chain.joint(2).rot() = Gen::rot(Biv::xy * amt2);
chain.joint(3).rot() = Gen::rot(Biv::xy * amt3);
chain.fk();
Draw(chain);
for (int i=1;i<chain.num();++i){
// Draw(chain.prevCircle(i),1,0,0);
// Rot localunlink = chain[i].rot() * !chain.link(i-1).rot();
// Vec rj = Op::rj( Vec(chain[i].pos() - chain[i-1].pos()), Biv::xy.spin( localunlink) );
// DrawAt( Vec::z.spin( chain.prevRot(i) ), chain[i].pos(),1,0,1);
//
auto target = ( chain[i].vec() - chain[i-1].vec() ).unit(); //global target direction
auto linkRot = Gen::ratio(Vec::y, chain.link(i-1).vec().unit() ); //what link position contributes...
auto trot = chain[i-1].rot() * linkRot;
// mFrame[i].rot() = mFrame[i-1].rot() * mLink[i-1].rot();
target = target.spin( !(trot) );
// auto rot = Gen::ratio( Vec::y.spin(trot), target ) * chain[i-1].rot();
auto adj = Gen::ratio( Vec::y, target );
// DrawAt( Vec::z.spin(rot), chain[i].pos(), 0,1,1);
DrawAt( Vec::y.spin( chain[i-1].rot ), chain[i-1].pos(), 0,1,0);
}
}
virtual void onDraw(){
getMouse();
Draw(mouse);
static Chain chain(5);
chain.fabrik( mouse, 4, 0 );
chain.fk();
for (int i = 0; i < chain.num(); ++i){
Draw( chain[i] );
//Touch( interface, chain.joint(i), chain[i] );
}
}
