void SRS::SolveAim(float psi_angle, Matrix R1)
{
float h1[3], N[3], angle;
Matrix S0, S1;
// Get the final hand position
evalcircle(c, u, v, radius, psi_angle, h1);
// Rotate ee_r1 to h1
crossproduct(N, ee_r1, h1);
unitize(N);
angle = angle_between_vectors(ee_r1, h1, N);
rotation_axis_to_matrix(N, angle, S0);
// Now rotate a0 to a
float a[3], a0[3];
vecsub(a, (float*)ee, h1);
unitize(a);
hmatmult(S1,Ry,S0);
vecmult0(a0, (float*)axis, S1);
cpvector(N, h1);
unitize(N);
angle = angle_between_vectors(a0, a, N);
rotation_axis_to_matrix(N, angle, S1);
hmatmult(R1, S0, S1);
}
float SRS::PosToAngle(const float p[3])
{
// Find vector from center of circle to pos and project it onto circle
float cp[3], pp[3];
vecsub(cp, (float *) p, c);
project_plane(pp , cp, n);
// Find angle between u and pp. This is the swivel angle
return angle_between_vectors(u, pp, n);
}
/*****************************************************************
double* excess_of_quad(int ni, int nj, double *vec1, double *vec2,
double *vec3, double *vec4 )
*******************************************************************/
double* excess_of_quad(int ni, int nj, double *vec1, double *vec2, double *vec3, double *vec4 )
{
int n;
double ang12, ang23, ang34, ang41;
double *excess, *plane1, *plane2, *plane3, *plane4;
double *angle12, *angle23, *angle34, *angle41;
excess = (double *)malloc(ni*nj*sizeof(double));
plane1=plane_normal(ni, nj, vec1, vec2);
plane2=plane_normal(ni, nj, vec2, vec3);
plane3=plane_normal(ni, nj, vec3, vec4);
plane4=plane_normal(ni, nj, vec4, vec1);
angle12=angle_between_vectors(ni, nj, plane2,plane1);
angle23=angle_between_vectors(ni, nj, plane3,plane2);
angle34=angle_between_vectors(ni, nj, plane4,plane3);
angle41=angle_between_vectors(ni, nj, plane1,plane4);
for(n=0; n<ni*nj; n++) {
ang12 = M_PI-angle12[n];
ang23 = M_PI-angle23[n];
ang34 = M_PI-angle34[n];
ang41 = M_PI-angle41[n];
excess[n] = ang12+ang23+ang34+ang41-2*M_PI;
}
free(plane1);
free(plane2);
free(plane3);
free(plane4);
free(angle12);
free(angle23);
free(angle34);
free(angle41);
return excess;
}; /* excess_of_quad */
void CBurer::face_enemy ()
{
if ( !EnemyMan.get_enemy() )
{
return;
}
Fvector const enemy_pos = EnemyMan.get_enemy()->Position();
Fvector const self_pos = Position();
Fvector const self2enemy = enemy_pos - self_pos;
bool const good_aiming = angle_between_vectors(self2enemy, Direction())
< deg2rad(20.f);
if ( !good_aiming )
{
dir().face_target (enemy_pos);
}
set_action (ACT_STAND_IDLE);
}
void calc_fvgrid(int nx, int ny, int nratio, double *dx, double *dy, double *area)
{
int nxf, nyf, nif, njf, nxp, nyp, nxh, nyh, i, j, n, m;
double ar;
double *lx, *ly, *X, *Y, *Z, *vec1, *vec2, *vec3, *vec4, *dxl, *dyl, *areal;
size_t *dims;
nxp = nx+1;
nyp = ny+1;
nxh = (nxp+1)/2;
nyh = (nyp+1)/2;
nxf = nx*nratio+1;
nyf = nxf;
nif = (nxf+1)/2;
njf = nif;
lx = (double *)malloc(nif*njf*sizeof(double));
ly = (double *)malloc(nif*njf*sizeof(double));
n = 0;
for(j=0; j<njf; j++) {
for(i=0; i<nif; i++) {
lx[n] = -1. + 2.0*i/(nxf-1);
ly[n++] = -1. + 2.0*j/(nyf-1);
}
}
X = (double *)malloc(nif*njf*sizeof(double));
Y = (double *)malloc(nif*njf*sizeof(double));
Z = (double *)malloc(nif*njf*sizeof(double));
/* calculating 3D coordinates on unit sphere */
conformal_map_coords2xyz( nif, njf, lx, ly, X, Y, Z);
vec1 = (double *)malloc((nif-1)*njf*3*sizeof(double));
vec2 = (double *)malloc((nif-1)*njf*3*sizeof(double));
for(j=0;j<njf;j++) {
for(i=0;i<nif-1;i++) {
n = j*(nif-1)+i;
vec1[n] = X[j*nif+i];
vec2[n] = X[j*nif+i+1];
vec1[(nif-1)*njf+n] = Y[j*nif+i];
vec2[(nif-1)*njf+n] = Y[j*nif+i+1];
vec1[2*(nif-1)*njf+n] = Z[j*nif+i];
vec2[2*(nif-1)*njf+n] = Z[j*nif+i+1];
}
}
dxl = angle_between_vectors( nif-1, njf, vec1, vec2);
free(vec1);
free(vec2);
vec1 = (double *)malloc(nif*(njf-1)*3*sizeof(double));
vec2 = (double *)malloc(nif*(njf-1)*3*sizeof(double));
for(j=0;j<njf-1;j++) {
for(i=0;i<nif;i++) {
n = j*nif+i;
vec1[n] = X[j*nif+i];
vec2[n] = X[(j+1)*nif+i];
vec1[nif*(njf-1)+n] = Y[j*nif+i];
vec2[nif*(njf-1)+n] = Y[(j+1)*nif+i];
vec1[2*nif*(njf-1)+n] = Z[j*nif+i];
vec2[2*nif*(njf-1)+n] = Z[(j+1)*nif+i];
}
}
dyl = angle_between_vectors( nif, njf-1, vec1, vec2);
free(vec1);
free(vec2);
vec1 = (double *)malloc((nif-1)*(njf-1)*3*sizeof(double));
vec2 = (double *)malloc((nif-1)*(njf-1)*3*sizeof(double));
vec3 = (double *)malloc((nif-1)*(njf-1)*3*sizeof(double));
vec4 = (double *)malloc((nif-1)*(njf-1)*3*sizeof(double));
for(j=0;j<njf-1;j++) {
for(i=0;i<nif-1;i++) {
n = j*(nif-1)+i;
vec1[n] = X[j*nif+i];
vec2[n] = X[j*nif+i+1];
vec3[n] = X[(j+1)*nif+i+1];
vec4[n] = X[(j+1)*nif+i];
vec1[(nif-1)*(njf-1)+n] = Y[j*nif+i];
vec2[(nif-1)*(njf-1)+n] = Y[j*nif+i+1];
vec3[(nif-1)*(njf-1)+n] = Y[(j+1)*nif+i+1];
vec4[(nif-1)*(njf-1)+n] = Y[(j+1)*nif+i];
vec1[2*(nif-1)*(njf-1)+n] = Z[j*nif+i];
vec2[2*(nif-1)*(njf-1)+n] = Z[j*nif+i+1];
vec3[2*(nif-1)*(njf-1)+n] = Z[(j+1)*nif+i+1];
vec4[2*(nif-1)*(njf-1)+n] = Z[(j+1)*nif+i];
}
}
areal = excess_of_quad( nif-1, njf-1, vec1, vec2, vec3, vec4);
free(vec1);
free(vec2);
free(vec3);
free(vec4);
/*Force some symmetry (probably does nothing) */
for(j=0; j<njf; j++) {
for(i=0; i<nif-1; i++) dxl[j*(nif-1)+i] = (dxl[j*(nif-1)+i]+dyl[i*nif+j])*0.5;
}
for(j=0; j<njf-1; j++) {
for(i=0; i<nif; i++) dyl[j*nif+i] = dxl[i*(nif-1)+j];
}
for(j=0; j<njf-1; j++) {
for(i=0; i<nif-1; i++) {
if(j<i)
areal[j*(nif-1)+i] = (areal[j*(nif-1)+i]+areal[i*(nif-1)+j])*0.5;
else if(j>i)
areal[j*(nif-1)+i] = areal[i*(nif-1)+j];
}
}
/* Use symmetry to fill second octant */
for(j=1; j<njf; j++) {
for(i=0; i<j; i++) {
dxl[j*(nif-1)+i] = dyl[i*nif+j];
}
}
for(j=1; j<njf-1; j++) {
for(i=0; i<j; i++) {
areal[j*(nif-1)+i] = areal[i*(nif-1)+j];
}
}
/* copy data from fine grid to super grid. */
for(j=0;j<nyh;j++) {
for(i=0;i<nxh-1;i++) {
ar = 0;
for(n=0;n<nratio;n++) ar = ar + dxl[j*nratio*(nif-1)+i*nratio+n];
dx[j*nx+i] = ar*RADIUS;
}
}
for(j=0;j<nyh-1;j++) {
for(i=0;i<nxh-1;i++) {
ar = 0;
for(n=0; n<nratio; n++)
for(m=0; m<nratio; m++) ar += areal[(j*nratio+n)*(nif-1)+i*nratio+m];
area[j*nx+i] = ar*RADIUS*RADIUS;
}
}
/*use reflection symmetry of quadrants to fill face. */
for(j=0;j<nyh; j++) {
for(i=nxh-1; i<nx; i++) {
dx[j*nx+i] = dx[j*nx+nxp-i-2];
}
}
for(j=nyh;j<nyp; j++) {
for(i=0; i<nx; i++) {
dx[j*nx+i] = dx[(ny-j)*nx+i];
}
}
for(j=0;j<nyh-1; j++) {
for(i=nxh-1; i<nx; i++) {
area[j*nx+i] = area[j*nx+nxp-i-2];
}
}
for(j=nyh-1;j<ny; j++) {
for(i=0; i<nx; i++) {
area[j*ny+i] = area[(nyp-j-2)*nx+i];
}
}
/* copy dx to dy */
for(j=0;j<ny;j++)
for(i=0;i<nxp;i++) dy[j*nxp+i] = dx[i*nx+j] ;
}; /* calc_fvgrid */
