/// Deadzone Damped Pseudoinverse
void aa_la_dzdpinv( size_t m, size_t n, double s2_min, const double *A, double *A_star ) {
// A^T (AA^T + kI)^{-1}
// A is m*n
// x = Aq, x is m, q is n
const int mi = (int)m;
const int ni = (int)n;
// This method uses the SVD
double *W = (double*)aa_mem_region_local_alloc( sizeof(double) *
(m*m + n*n + AA_MIN(m,n)) );
double *U = W; // size m*m
double *Vt = U + m*m; // size n*n
double *S = Vt + n*n; // size min(m,n)
// A = U S V^T
aa_la_svd(m,n,A,U,S,Vt);
memset( A_star, 0, sizeof(double)*m*n );
// \sum s_i/(s_i**2+k) * v_i * u_i^T
for( size_t i = 0; i < AA_MIN(m,n); i ++ ) {
double s2 = AA_MAX( (S[i]*S[i]), s2_min );
cblas_dger( CblasColMajor, ni, mi, S[i] / s2,
Vt + i, ni,
U + m*i, 1,
A_star, ni
);
}
aa_mem_region_local_pop( W );
}
AA_API void
aa_rx_sg_sub_center_configs( const struct aa_rx_sg_sub *ssg,
size_t n, double *q )
{
size_t n_qs = aa_rx_sg_sub_config_count(ssg);
size_t n_min = AA_MIN(n,n_qs);
for( size_t i = 0; i < n_min; i ++ ) {
double min=0 ,max=0;
aa_rx_config_id config_id = aa_rx_sg_sub_config(ssg, i);
int r = aa_rx_sg_get_limit_pos( ssg->scenegraph, config_id, &min, &max );
if( 0 == r ) {
q[i] = (max + min) / 2;
} else {
q[i] = 0;
}
}
}
double aa_la_min( size_t n, const double *x ) {
double v = HUGE_VAL;
for( size_t i = 0; i < n; i++ )
v = AA_MIN(v,x[i]);
return v;
}