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); }