int multiply_matrix_vector (MATRIX const *m, VECTOR const *v, VECTOR *r)
/*****************************************************************************
Returns the VECTOR-MATRIX product of m and v in r.
******************************************************************************/
{
if (! (MV_COMPAT_DIM (*m, *v)))
{
printf ("ERROR (multiply_matrix_vector): MATRIX and VECTOR dimensions incompatible!\n");
print_matrix ("MATRIX:", m);
print_vector ("VECTOR:", v);
return -1; /*added 1996-07*/
}
else
{
int i, j;
float datum;
VELEMENTS (*r) = MROWS (*m);
for (i = 0; i < MROWS (*m); i++)
{
datum = 0;
for (j = 0; j < VELEMENTS (*v); j++)
datum = datum + MDATA (*m, i, j) * VDATA (*v, j);
VDATA (*r, i) = datum;
}
}
return 1;
}
static char *
new_symbol_string (bfd *abfd, const char *name)
{
char *n = VDATA (abfd)->strings;
strcpy (VDATA (abfd)->strings, name);
VDATA (abfd)->strings += strlen (VDATA (abfd)->strings) + 1;
return n;
}
void initialize_vector (VECTOR *v, int elements)
/*****************************************************************************
Initializes a VECTOR to dimension (elements) and its contents to zeros.
******************************************************************************/
{
VELEMENTS (*v) = elements;
{
int i;
for (i = 0; i < VELEMENTS (*v); i++)
VDATA (*v, i) = 0;
}
}
static asymbol *
versados_new_symbol (bfd *abfd,
int snum,
const char *name,
bfd_vma val,
asection *sec)
{
asymbol *n = VDATA (abfd)->symbols + snum;
n->name = name;
n->value = val;
n->section = sec;
n->the_bfd = abfd;
n->flags = 0;
return n;
}
void print_vector (char *message, VECTOR const *v)
/*****************************************************************************
Print to stdout the contents of VECTOR m.
******************************************************************************/
{
int i;
printf ("%s\n",message);
if (VELEMENTS (*v) <= MAX_ROWS)
for (i = 0; i < VELEMENTS (*v); i++)
{
printf ("%f ", VDATA (*v, i));
printf ("\n");
}
else printf ("Dimension incorrecta!");
printf ("\n");
}
static bfd_boolean
versados_mkobject (bfd *abfd)
{
if (abfd->tdata.versados_data == NULL)
{
bfd_size_type amt = sizeof (tdata_type);
tdata_type *tdata = bfd_alloc (abfd, amt);
if (tdata == NULL)
return FALSE;
abfd->tdata.versados_data = tdata;
tdata->symbols = NULL;
VDATA (abfd)->alert = 0x12345678;
}
bfd_default_set_arch_mach (abfd, bfd_arch_m68k, 0);
return TRUE;
}
VECTOR create_vector (int elements)
/*****************************************************************************
Initializes a VECTOR to dimension (elements) and its contents to zeros.
******************************************************************************/
{
VECTOR v;
VELEMENTS (v) = elements;
{
int i;
for (i = 0; i < VELEMENTS (v); i++)
VDATA (v, i) = 0;
}
return v;
}
static void
process_esd (bfd *abfd, struct ext_esd *esd, int pass)
{
/* Read through the ext def for the est entries. */
int togo = esd->size - 2;
bfd_vma size;
bfd_vma start;
asection *sec;
char name[11];
unsigned char *ptr = esd->esd_entries;
unsigned char *end = ptr + togo;
while (ptr < end)
{
int scn = *ptr & 0xf;
int typ = (*ptr >> 4) & 0xf;
/* Declare this section. */
sprintf (name, "%d", scn);
sec = bfd_make_section_old_way (abfd, strdup (name));
sec->target_index = scn;
EDATA (abfd, scn).section = sec;
ptr++;
switch (typ)
{
default:
abort ();
case ESD_XREF_SEC:
case ESD_XREF_SYM:
{
int snum = VDATA (abfd)->ref_idx++;
get_10 (&ptr, name);
if (pass == 1)
VDATA (abfd)->stringlen += strlen (name) + 1;
else
{
int esidx;
asymbol *s;
char *n = new_symbol_string (abfd, name);
s = versados_new_symbol (abfd, snum, n, (bfd_vma) 0,
bfd_und_section_ptr);
esidx = VDATA (abfd)->es_done++;
RDATA (abfd, esidx - ES_BASE) = s;
}
}
break;
case ESD_ABS:
size = get_4 (&ptr);
start = get_4 (&ptr);
break;
case ESD_STD_REL_SEC:
case ESD_SHRT_REL_SEC:
sec->size = get_4 (&ptr);
sec->flags |= SEC_ALLOC;
break;
case ESD_XDEF_IN_ABS:
sec = (asection *) & bfd_abs_section;
case ESD_XDEF_IN_SEC:
{
int snum = VDATA (abfd)->def_idx++;
bfd_vma val;
get_10 (&ptr, name);
val = get_4 (&ptr);
if (pass == 1)
/* Just remember the symbol. */
VDATA (abfd)->stringlen += strlen (name) + 1;
else
{
asymbol *s;
char *n = new_symbol_string (abfd, name);
s = versados_new_symbol (abfd, snum + VDATA (abfd)->nrefs, n,
val, sec);
s->flags |= BSF_GLOBAL;
}
}
break;
}
}
}
static int computeMatrixLMSOpt(TAsoc *cp_ass, int cnt, Tsc *estimacion) {
int i;
float LMETRICA2;
float X1[MAXLASERPOINTS], Y1[MAXLASERPOINTS];
float X2[MAXLASERPOINTS],Y2[MAXLASERPOINTS];
float X2Y2[MAXLASERPOINTS],X1X2[MAXLASERPOINTS];
float X1Y2[MAXLASERPOINTS], Y1X2[MAXLASERPOINTS];
float Y1Y2[MAXLASERPOINTS];
float K[MAXLASERPOINTS], DS[MAXLASERPOINTS];
float DsD[MAXLASERPOINTS], X2DsD[MAXLASERPOINTS], Y2DsD[MAXLASERPOINTS];
float Bs[MAXLASERPOINTS], BsD[MAXLASERPOINTS];
float A1, A2, A3, B1, B2, B3, C1, C2, C3, D1, D2, D3;
MATRIX matA,invMatA;
VECTOR vecB,vecSol;
A1=0;A2=0;A3=0;B1=0;B2=0;B3=0;
C1=0;C2=0;C3=0;D1=0;D2=0;D3=0;
LMETRICA2=params.LMET*params.LMET;
for (i=0; i<cnt; i++){
X1[i]=cp_ass[i].nx*cp_ass[i].nx;
Y1[i]=cp_ass[i].ny*cp_ass[i].ny;
X2[i]=cp_ass[i].rx*cp_ass[i].rx;
Y2[i]=cp_ass[i].ry*cp_ass[i].ry;
X2Y2[i]=cp_ass[i].rx*cp_ass[i].ry;
X1X2[i]=cp_ass[i].nx*cp_ass[i].rx;
X1Y2[i]=cp_ass[i].nx*cp_ass[i].ry;
Y1X2[i]=cp_ass[i].ny*cp_ass[i].rx;
Y1Y2[i]=cp_ass[i].ny*cp_ass[i].ry;
K[i]=X2[i]+Y2[i] + LMETRICA2;
DS[i]=Y1Y2[i] + X1X2[i];
DsD[i]=DS[i]/K[i];
X2DsD[i]=cp_ass[i].rx*DsD[i];
Y2DsD[i]=cp_ass[i].ry*DsD[i];
Bs[i]=X1Y2[i]-Y1X2[i];
BsD[i]=Bs[i]/K[i];
A1=A1 + (1-Y2[i]/K[i]);
B1=B1 + X2Y2[i]/K[i];
C1=C1 + (-cp_ass[i].ny + Y2DsD[i]);
D1=D1 + (cp_ass[i].nx - cp_ass[i].rx -cp_ass[i].ry*BsD[i]);
A2=B1;
B2=B2 + (1-X2[i]/K[i]);
C2=C2 + (cp_ass[i].nx-X2DsD[i]);
D2=D2 + (cp_ass[i].ny -cp_ass[i].ry +cp_ass[i].rx*BsD[i]);
A3=C1;
B3=C2;
C3=C3 + (X1[i] + Y1[i] - DS[i]*DS[i]/K[i]);
D3=D3 + (Bs[i]*(-1+DsD[i]));
}
initialize_matrix(&matA,3,3);
MDATA(matA,0,0)=A1; MDATA(matA,0,1)=B1; MDATA(matA,0,2)=C1;
MDATA(matA,1,0)=A2; MDATA(matA,1,1)=B2; MDATA(matA,1,2)=C2;
MDATA(matA,2,0)=A3; MDATA(matA,2,1)=B3; MDATA(matA,2,2)=C3;
if (inverse_matrix (&matA, &invMatA)==-1)
return -1;
#ifdef INTMATSM_DEB
print_matrix("inverted matrix", &invMatA);
#endif
initialize_vector(&vecB,3);
VDATA(vecB,0)=D1; VDATA(vecB,1)=D2; VDATA(vecB,2)=D3;
multiply_matrix_vector (&invMatA, &vecB, &vecSol);
estimacion->x=-VDATA(vecSol,0);
estimacion->y=-VDATA(vecSol,1);
estimacion->tita=-VDATA(vecSol,2);
return 1;
}
