void twoLinkArm::ikinAbs(point x, point &q)
{
x=x-params.x0;
double S=(std::pow(params.l1+params.l2,2)-(std::pow(x.X(),2)+std::pow(x.Y(),2)))/(std::pow(x.X(),2)+std::pow(x.Y(),2)-std::pow(params.l1-params.l2,2));
q[1]=-2.0*std::atan(std::sqrt(std::abs(S))); //Negative sign indicates elbow right.
q[0]=std::atan2(x.Y(),x.X())-std::atan2(params.l2*std::sin(q[1]),params.l1+params.l2*std::cos(q[1]));
}
bool twoLinkArm::ikin(point x, point &q)
{
x=x-params.x0;
double S=(std::pow(params.l1+params.l2,2)-(std::pow(x.X(),2)+std::pow(x.Y(),2)))/(std::pow(x.X(),2)+std::pow(x.Y(),2)-std::pow(params.l1-params.l2,2));
if(S<0) {q=point(); return false;}
q[1]=-2.0*std::atan(std::sqrt(S)); //Negative sign indicates elbow right.
q[0]=std::atan2(x.Y(),x.X())-std::atan2(params.l2*std::sin(q[1]),params.l1+params.l2*std::cos(q[1]));
return true;
}
void twoLinkArm::computeInertiaCoriolis(point q, point qdot, mat2 &D, point &C)
{
double c2=std::cos(q.Y());
double d11=params.m1*std::pow(params.lc1,2)+params.m2*(std::pow(params.l1,2)+std::pow(params.lc2,2)+2.0*params.l1*params.lc2*c2)+params.I1+params.I2;
double d12=params.m2*(std::pow(params.lc2,2)+params.l1*params.lc2*c2)+params.I2;
double d21=d12;
double d22=params.m2*std::pow(params.lc2,2)+params.I2;
double h=params.m2*params.l1*params.lc2*std::sin(q.Y());
D=mat2(d11,d12,d21,d22);
C=point(h*qdot.Y()*(2*qdot.X()+qdot.Y()), h*std::pow(qdot.X(),2));
}
point TargetControl::genTarget(point currentPos, bool directionMatters)
{
double theta;
point targetPos;
if(!directionMatters) //Direction not constrained this trial
{
do
{
theta=randb(0,360);
targetPos=currentPos+point(cos(theta),sin(theta))*spawnDist;
} while(targetPos.mag()>innerRadius);
return targetPos;
}
//If direction predetermined, use it
if (nextDirection>=0) {targetPos=currentPos+point(cos(nextDirection),sin(nextDirection))*spawnDist; nextDirection=-1; return targetPos;}
//Make sure we haven't bottomed out
int sum=0;
for(int k=0;k<directions;k++)
{
sum+=((timesUsed[k]>=minUses)?0:1);
}
if (sum==0) minUses++;
//Find the best direction
double min=std::numeric_limits<double>::infinity();
int index=-1;
double mag;
point bestPos;
for(int k=0;k<directions;k++)
{
targetPos=currentPos+point(cos(direction[k]),sin(direction[k]))*spawnDist;
mag=targetPos.mag();
if((mag<min)&&(timesUsed[k]<minUses)&&(mag<innerRadius)) {min=mag; index=k; bestPos=targetPos;}
}
if(index != -1)
{
timesUsed[index]++;
return bestPos;
}
//If no direction can produce a legal target pick randomly among the least used and set up for it
int m=INT_MAX;
int i;
int * shuffle = new int[directions];
shuffle[0]=directions;
randperm(shuffle);
for(int k=0;k<directions;k++)
if(timesUsed[shuffle[k]]<m) {m=timesUsed[shuffle[k]]; i=shuffle[k];}
timesUsed[i]++;
nextDirection=direction[i];
theta=atan2(currentPos.Y()+spawnDist*sin(direction[i]),currentPos.X()+spawnDist*cos(direction[i]));
targetPos=currentPos - point(cos(theta),sin(theta))*spawnDist;
return (targetPos.mag()<=maxRadius)?targetPos:point(0,0);
}
mat2 twoLinkArm::jacobian(point q)
{
double s12=std::sin(q.X()+q.Y());
double c12=std::cos(q.X()+q.Y());
double fJ11=-params.l1*std::sin(q.X())-params.l2*s12;
double fJ21=params.l1*std::cos(q.X())+params.l2*c12;
double fJ12=-params.l2*s12;
double fJ22=params.l2*c12;
return mat2(fJ11,fJ12,fJ21,fJ22);
}
bool Frustum::InterSect( point& p )
{
std::vector<Plane> planes = FormPlane();
for (unsigned int i = 0;i<planes.size();i++)
{
if(planes[i].A*p.X()+planes[i].B*p.Y()+planes[i].C*p.Z()+planes[i].D > 0.f)
{
return false;
}
}
return true;
}
unsigned int Frustum::InterSectClip( point& p )
{
unsigned ret = 0;
std::vector<Plane> planes = FormPlane();
for (unsigned int i = 0;i<planes.size();i++)
{
if(planes[i].A*p.X()+planes[i].B*p.Y()+planes[i].C*p.Z()+planes[i].D > 0.f)
{
ret |= 1<<i;
}
}
return ret;
}
point twoLinkArm::getQDDot(point q, point qdot, point xddot)
{
double s12=std::sin(q.X()+q.Y());
double c12=std::cos(q.X()+q.Y());
double fJt1dx2=-params.l1*std::cos(q.X())-params.l2*c12;
double fJt1dy2=-params.l2*std::cos(q.X()+q.Y());
double fJt2dx2=-params.l1*std::sin(q.X())-params.l2*s12;
double fJt2dy2=-params.l2*s12;
double fJt1dxdy=-params.l2*c12;
double fJt2dxdy=-params.l2*s12;
return jacobian(q)/(xddot-(mat2(fJt1dx2*qdot.X()+fJt1dxdy*qdot.Y(),fJt1dy2*qdot.Y()+fJt1dxdy*qdot.X(),
fJt2dx2*qdot.X()+fJt2dxdy*qdot.Y(),fJt2dy2*qdot.Y()+fJt2dxdy*qdot.X())*qdot));
}
void DisplayWidget::calibrate(point Center, point probe1, point probe2)
{
center=Center;
double rot=std::atan2(probe1.Y()-center.Y(),probe1.X()-center.X())-std::atan2(0,LEFTPROBE); //In theory, any one point is enough
screenRotation=(180l/M_PI)*rot; //OpenGL likes degrees.
point one=(probe1-center).rotateZero(-rot);
point two=(probe2-center).rotateZero(-rot);
std::cout << rot << " " << one.X() << " " << one.Y() << " " << two.X() << " " << two.Y() << std::endl; //Why not?
width=SCREENWIDTH/LEFTPROBE*one.X();
height=SCREENHEIGHT/UPPROBE*two.Y();
calibrationMode=false;
std::cout << width << " " << height << std::endl; //Why not?
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
glOrtho(-width/2l+center.X(),width/2l+center.X(),-height/2l+center.Y(),height/2l+center.Y(),MINDEPTH,MAXDEPTH);
glTranslated(center.X(),center.Y(),0);
glRotated(fmod(screenRotation,180),0,0,1);
glTranslated(-center.X(),-center.Y(),0);
}
float_type diffy(const point<float_type, dim>& op1, const point<float_type, dim>& op2)
{
return op2.Y() - op1.Y();
}
void twoLinkArm::fkin(point q, point &x, point &x1)
{
x1=point(params.x0.X()+params.l1*std::cos(q.X()),params.x0.Y()+params.l1*std::sin(q.X()));
x=x1+point(params.l2*std::cos(q.X()+q.Y()),params.l2*std::sin(q.X()+q.Y()));
}
point twoLinkArm::fkin(point q)
{
return point(params.x0.X()+params.l1*std::cos(q.X())+params.l2*std::cos(q.X()+q.Y()),
params.x0.Y()+params.l1*std::sin(q.X())+params.l2*std::sin(q.X()+q.Y()));
}
point TargetControl::genTarget(point currentPos, bool directionMatters)
{
double theta;
point targetPos;
if(!directionMatters) //Direction not constrained this trial
{
do
{
theta=randb(0,360);
targetPos=currentPos+point(cos(theta),sin(theta))*spawnDist;
} while(targetPos.mag()>innerRadius);
return targetPos;
}
//If direction predetermined, use it
if (nextDirection>=0) {targetPos=currentPos+point(cos(nextDirection),sin(nextDirection))*spawnDist; nextDirection=-1; return targetPos;}
//Make sure we haven't bottomed out
int sum=0;
for(int k=0;k<directions;k++)
{
sum+=((timesUsed[k]>=minUses)?0:1);
}
if (sum==0) minUses++;
//Find the best direction
double min=std::numeric_limits<double>::infinity();
int index=-1;
double mag;
point bestPos;
for(int k=0;k<directions;k++)
{
targetPos=currentPos+point(cos(direction[k]),sin(direction[k]))*spawnDist;
mag=targetPos.mag();
if((mag<min)&&(timesUsed[k]<minUses)&&(mag<innerRadius)) {min=mag; index=k; bestPos=targetPos;}
}
if(index != -1)
{
timesUsed[index]++;
return bestPos;
}
//If no direction can produce a legal target pick randomly among the least used and set up for it
int m=INT_MAX;
int i;
int * shuffle = new int[directions];
randperm(shuffle,directions);
for(int k=0;k<directions;k++)
if(timesUsed[shuffle[k]]<m) {m=timesUsed[shuffle[k]]; i=shuffle[k];}
timesUsed[i]++;
nextDirection=direction[i];
theta=atan2(currentPos.Y()+spawnDist*sin(direction[i]),currentPos.X()+spawnDist*cos(direction[i]))-3.14159265;
targetPos=currentPos - point(cos(theta),sin(theta))*spawnDist;
if (targetPos.mag()>innerRadius)
{
point V=point(spawnDist*cos(direction[i]),spawnDist*sin(direction[i]));
point center=currentPos+V;
double distance=center.mag();
double xi=(pow(distance,2)+pow(innerRadius,2)-pow(spawnDist,2))/(2*distance);
double yi=sqrt(pow(innerRadius,2)-pow(xi,2));
point i1=center*(xi/distance)+point(-center.Y(),center.X())*(yi/distance);
point i2=center*(xi/distance)-point(-center.Y(),center.X())*(yi/distance);
double angle1=atan2(i1.Y()-V.Y()-currentPos.Y(),i1.X()-V.X()-currentPos.X());
double angle2=atan2(i2.Y()-V.Y()-currentPos.Y(),i2.X()-V.X()-currentPos.X());
point p1p=currentPos+point(cos(angle1),sin(angle1))*spawnDist;
point p1m=currentPos+point(cos(angle2),sin(angle2))*spawnDist;
if (p1p.mag()>p1m.mag()) targetPos=p1m;
else targetPos=p1p;
}
return (targetPos.mag()<=maxRadius)?targetPos:point(0,0);
}
