// Solve angles!
bool solve(float x, float y, float z, float& a0, float& a1, float& a2)
{
// Solve top-down view
float r, th0;
cart2polar(y, x, r, th0);
// Account for the wrist length!
r -= L3;
// In arm plane, convert to polar
float ang_P, R;
cart2polar(r, z, R, ang_P);
// Solve arm inner angles as required
float B, C;
if(!cosangle(L2, L1, R, B)) return false;
if(!cosangle(R, L1, L2, C)) return false;
// Solve for servo angles from horizontal
a0 = th0;
a1 = ang_P + B;
a2 = C + a1 - PI;
return true;
}
VectorXd CircleOrientationError::operator()(const VectorXd& a) const {
assert(a.size() == 6);
Matrix4d current_pose = cfg->pose * expUp(a);
Vector3d current_rot = current_pose.topLeftCorner<3, 3>() * Vector3d(0, 0, 1);
Vector3d target_rot = target_pose.topLeftCorner<3, 3>() * Vector3d(0, 0, 1);
//double orientation_error = fmax(1 - cosangle(current_rot, target_rot) - this->orientation_error_relax, 0);
double orientation_error = 1 - cosangle(current_rot, target_rot) - this->orientation_error_relax;
VectorXd err(1);
err << orientation_error;
return err;
}
char *test_angle() {
mu_assert("angle fails", equal(-0.70706, cosangle(1, 0, -1, 1)));
mu_assert("angle fails", equal(cos(15 * PI / 180), cosangle(1, 1, 0.866, 0.5)));
return 0;
}
void proj_rect_grid(rmap_t *mapin, double thetax, double thetay,
const loc_t *locout,const double ratiox, const double ratioy,
const double *ampout, double* phiout,
double sc, double hs, double ht,
double betax, double betay){
/*
input parameters:
mapin: M3 surface map, NOT OPD.
thetax, thetay: tilt of surface map along x or y. one has to be pi/2
locout: Pupil plan opd grid.
ratio: scaling of pupil plan to exit pupil plane.
sc: scaling of opd. don't include projection
hs: distance from exit pupil to m3.
betax,betay: beam angle from exit pupil to focal plane.
*/
const double (*phiin)[mapin->nx]=(void*)mapin->p;
const int wrapx1 = mapin->nx;
const int wrapy1 = mapin->ny;
const int wrapx = wrapx1-1;
const int wrapy = wrapy1-1;
double offx=hs*betax;
double offy=hs*betay;
if(ratiox<0) offx=-offx;
if(ratioy<0) offy=-offy;
const double dx_in1 = 1./mapin->dx;
const double dy_in1 = 1./mapin->dy;
double a0x=(-offx/sin(thetax)-mapin->ox)*dx_in1;
double a0y=(-offy/sin(thetay)-mapin->oy)*dy_in1;
double ddx=(hs-ht)*dx_in1;
double ddy=(hs-ht)*dy_in1;
int nplocx,nplocy,nplocx1,nplocy1;
if(fabs(thetax-M_PI*0.5)>M_PI*.45 || fabs(thetay-M_PI*0.5)>M_PI*0.45){
info2("Tilting angle is too much\n");
return;
}
double vm3[3];
vm3[0]=-sin(M_PI/2-thetax);
vm3[1]=-sin(M_PI/2-thetay);
vm3[2]=-sqrt(1.-vm3[0]*vm3[0]-vm3[1]*vm3[1]);
double vi[3];
vi[2]=-hs;
double sc2;
for(int iloc=0; iloc<locout->nloc; iloc++){
if(ampout && fabs(ampout[iloc])<1.e-10)
continue;/*skip points that has zero amplitude */
double alx=atan2(locout->locx[iloc]*ratiox+offx,hs);
double aly=atan2(locout->locy[iloc]*ratioy+offy,hs);
double btx=thetax-alx;
double bty=thetay-aly;
double dplocx=ddx*sin(alx)/sin(btx)+a0x;
double dplocy=ddy*sin(aly)/sin(bty)+a0y;
vi[0]=locout->locx[iloc]*ratiox+offx;
vi[1]=locout->locy[iloc]*ratioy+offy;
SPLIT(dplocx,dplocx,nplocx);
SPLIT(dplocy,dplocy,nplocy);
if(nplocx<0||nplocx>=wrapx||nplocy<0||nplocy>=wrapy){
continue;
}else{
nplocx1=nplocx+1;
nplocy1=nplocy+1;
}
sc2=sc*cosangle(vi,vm3);
/*sc2=sc*0.707; */
phiout[iloc]+=sc2*(phiin[nplocy][nplocx]*(1.-dplocx)*(1.-dplocy)
+phiin[nplocy][nplocx1]*(dplocx)*(1.-dplocy)
+phiin[nplocy1][nplocx]*(1.-dplocx)*(dplocy)
+phiin[nplocy1][nplocx1]*(dplocx)*(dplocy));
}
}
// two vectors are in the inverse direction?
static bool inverseDirection(float vector1[3], float vector2[3])
{
// -sqrt(2) / 2
return cosangle(vector1, vector2) <= -0.707106;
}
// two vectors are in the same direction?
static bool sameDirection(float vector1[3], float vector2[3])
{
// sqrt(2) / 2
return cosangle(vector1, vector2) >= 0.707106;
}
