/*!
Least squares method used to make the tracking more robust. It
ensures that the points taken into account to compute the right
equation belong to the line.
*/
void
vpMeLine::leastSquare()
{
vpMatrix A(numberOfSignal(),2) ;
vpColVector x(2), x_1(2) ;
x_1 = 0;
unsigned int i ;
vpRobust r(numberOfSignal()) ;
r.setThreshold(2);
r.setIteration(0) ;
vpMatrix D(numberOfSignal(),numberOfSignal()) ;
D.eye() ;
vpMatrix DA, DAmemory ;
vpColVector DAx ;
vpColVector w(numberOfSignal()) ;
vpColVector B(numberOfSignal()) ;
w =1 ;
vpMeSite p_me ;
unsigned int iter =0 ;
unsigned int nos_1 = 0 ;
double distance = 100;
if (list.size() <= 2 || numberOfSignal() <= 2)
{
//vpERROR_TRACE("Not enough point") ;
vpCDEBUG(1) << "Not enough point";
throw(vpTrackingException(vpTrackingException::notEnoughPointError,
"not enough point")) ;
}
if ((fabs(b) >=0.9)) // Construction du systeme Ax=B
// a i + j + c = 0
// A = (i 1) B = (-j)
{
nos_1 = numberOfSignal() ;
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = p_me.ifloat ;
A[k][1] = 1 ;
B[k] = -p_me.jfloat ;
k++ ;
}
}
while (iter < 4 && distance > 0.05)
{
DA = D*A ;
x = DA.pseudoInverse(1e-26) *D*B ;
vpColVector residu(nos_1);
residu = B - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++ ;
distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
x_1 = x;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < 0.2)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
// mise a jour de l'equation de la droite
a = x[0] ;
b = 1 ;
c = x[1] ;
double s =sqrt( vpMath::sqr(a)+vpMath::sqr(b)) ;
a /= s ;
b /= s ;
c /= s ;
}
else // Construction du systeme Ax=B
// i + bj + c = 0
// A = (j 1) B = (-i)
{
nos_1 = numberOfSignal() ;
unsigned int k =0 ;
for(std::list<vpMeSite>::const_iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
A[k][0] = p_me.jfloat ;
A[k][1] = 1 ;
B[k] = -p_me.ifloat ;
k++ ;
}
}
while (iter < 4 && distance > 0.05)
{
DA = D*A ;
x = DA.pseudoInverse(1e-26) *D*B ;
vpColVector residu(nos_1);
residu = B - A*x;
r.setIteration(iter) ;
r.MEstimator(vpRobust::TUKEY,residu,w) ;
k = 0;
for (i=0 ; i < nos_1 ; i++)
{
D[k][k] =w[k] ;
k++;
}
iter++ ;
distance = fabs(x[0]-x_1[0])+fabs(x[1]-x_1[1]);
x_1 = x;
}
k =0 ;
for(std::list<vpMeSite>::iterator it=list.begin(); it!=list.end(); ++it){
p_me = *it;
if (p_me.getState() == vpMeSite::NO_SUPPRESSION)
{
if (w[k] < 0.2)
{
p_me.setState(vpMeSite::M_ESTIMATOR);
*it = p_me;
}
k++ ;
}
}
a = 1 ;
b = x[0] ;
c = x[1] ;
double s = sqrt(vpMath::sqr(a)+vpMath::sqr(b)) ;
a /= s ;
b /= s ;
c /= s ;
}
// mise a jour du delta
delta = atan2(a,b) ;
normalizeAngle(delta) ;
}
void BeamElement::CalculateSectionProperties()
{
KRATOS_TRY
array_1d<double, 3> x_0;
array_1d<double, 3> x_1; // Vector que contiene coordenadas de los nodos.
array_1d<double, 3> length; // Vector que contiene la direccion de la barra.
// double minimo, maximo, B;
// const double b = GetProperties()[BASE];
// const double h = GetProperties()[HEIGHT];
if( GetProperties().Has(CROSS_AREA) )
mArea = GetProperties()[CROSS_AREA];
else
mArea = GetValue(AREA);
Matrix* inertia;
if( GetProperties().Has(LOCAL_INERTIA) )
{
inertia = &(GetProperties()[LOCAL_INERTIA]);
}
else if( GetProperties().Has(INERTIA) )
{
inertia = &(GetProperties()[INERTIA]);
}
else if( Has(LOCAL_INERTIA) )
{
inertia = &(GetValue(LOCAL_INERTIA));
}
else if( Has(INERTIA) )
{
inertia = &(GetValue(INERTIA));
}
else
KRATOS_THROW_ERROR(std::logic_error, "The Inertia is not fully defined for the element", "")
mInertia_x = (*inertia)(0,0);
mInertia_y = (*inertia)(1,1);
mInertia_Polar = (*inertia)(0,1);
// mInertia_x = b * h * h * h / 12.0;
// mInertia_y = b * b * b * h / 12.0;
// minimo = std::min( b, h );
// maximo = std::max( b, h );
// B = ( 1.00 - 0.63 * ( minimo / maximo ) * ( 1 - ( pow( minimo, 4 ) / ( 12 * pow( maximo, 4 ) ) ) ) ) / 3; // constante torsional. Solo para secciones rectangulares.
// mInertia_Polar = B * minimo * minimo * minimo * maximo;
// mArea = b * h;
x_0( 0 ) = GetGeometry()[0].X0();
x_0( 1 ) = GetGeometry()[0].Y0();
x_0( 2 ) = GetGeometry()[0].Z0();
x_1( 0 ) = GetGeometry()[1].X0();
x_1( 1 ) = GetGeometry()[1].Y0();
x_1( 2 ) = GetGeometry()[1].Z0();
noalias( length ) = x_1 - x_0;
mlength = std::sqrt( inner_prod( length, length ) );
if (mlength == 0.00)
KRATOS_THROW_ERROR(std::invalid_argument, "Zero length found in elemnet #", this->Id());
KRATOS_CATCH( "" )
}
