/* v_mltadd -- scalar/vector multiplication and addition
-- out = v1 + scale.v2 */
VEC *v_mltadd(VEC *v1,VEC *v2,double scale,VEC *out)
{
/* register u_int dim, i; */
/* Real *out_ve, *v1_ve, *v2_ve; */
if ( v1==(VEC *)NULL || v2==(VEC *)NULL )
error(E_NULL,"v_mltadd");
if ( v1->dim != v2->dim )
error(E_SIZES,"v_mltadd");
if ( scale == 0.0 )
return v_copy(v1,out);
if ( scale == 1.0 )
return v_add(v1,v2,out);
if ( v2 != out )
{
tracecatch(out = v_copy(v1,out),"v_mltadd");
/* dim = v1->dim; */
__mltadd__(out->ve,v2->ve,scale,(int)(v1->dim));
}
else
{
tracecatch(out = sv_mlt(scale,v2,out),"v_mltadd");
out = v_add(v1,out,out);
}
/************************************************************
out_ve = out->ve; v1_ve = v1->ve; v2_ve = v2->ve;
for ( i=0; i < dim ; i++ )
out->ve[i] = v1->ve[i] + scale*v2->ve[i];
(*out_ve++) = (*v1_ve++) + scale*(*v2_ve++);
************************************************************/
return (out);
}
float KChainOrientationBodySchema::TryInverseKinematics(const cart_vec_t position){
int i,j;
float cand,rdist=0,tdist=0,k=0.0001; //rotation vs translation weight
CVector3_t stack[MAX_LINKS];
CVector3_t tar,newrod,rod,diff,pos;
CQuat_t q1,q2,q3,q4,rot;
// converting data format
v_copy(position.GetArray(),pos);
q_complete(position.GetArray()+3,rot);
q_inv(rot,q1);
for(i=0;i<nb_joints;i++){
GetQuaternion(i,q2);
q_multiply(q2,q1,q3);
q_copy(q3,q1);
}
for(j=0;j<50;j++){
#ifdef WITH_LAST_LINK
last_link->GetTranslation(rod);
#else
v_clear(rod);
#endif
InverseKinematicsStack(pos,stack);
for(i=nb_joints-1;i>=0;i--){
GetInverseQuaternion(i,q2);
q_multiply(q2,q1,q3); //q1*inv(Ri)
if(IsInverted(i)==-1){
v_copy(stack[i],tar);
Translate(i,rod,newrod); //getting to joint
cand = joints[i]->MinimizePositionAndRotationAngle(tar,newrod,q3,k);
cand = AngleInClosestRange(i,cand);
joints[i]->SetAngle(-cand);// todo to check if it is -
Rotate(i,newrod,rod); // updating "rod"
v_sub(tar,rod,diff);
}
else{
Rotate(i,stack[i+1],tar); //rotating back
cand = joints[i]->MinimizePositionAndRotationAngle(tar,rod,q3,k);
cand = AngleInClosestRange(i,cand);
joints[i]->SetAngle(cand);
Rotate(i,rod,newrod);
Translate(i,newrod,rod);
v_sub(tar,newrod,diff);
}
GetQuaternion(i,q2);
q_multiply(q3,q2,q1);
q_multiply(q2,q3,q4);
rdist = v_length(q4);//rotation distance, only the first 3 components
tdist = v_length(diff);//translation distance
// cout<<"rot "<<rdist<<" pos "<<tdist<<" prod: "<<(1-k)*rdist+k*tdist<<endl;
if(tdist<tol && rdist<rot_tol){return rdist/rot_tol+tdist;}
}
q_multiply(rot,q1,q2);
q_inv(rot,q3);
q_multiply(q2,q3,q1);
}
return rdist/rot_tol + tdist;
}
void Reaching::SetWeights(cart_vec_t& w_cart, joint_vec_t& w_angle){
v_copy(w_cart,weight_cart);
v4_copy(w_angle,weight_angle);
#ifdef DYN_WEIGHTS
v_copy(w_cart,base_weight_cart);
v4_copy(w_angle,base_weight_angle);
#endif
}
void Reaching::GetWeights(cart_vec_t& w_cart, joint_vec_t& w_angle)const{
#ifdef DYN_WEIGHTS
v4_copy(base_weight_angle,w_angle);
v_copy(base_weight_cart,w_cart);
#else
v4_copy(weight_angle,w_angle);
v_copy(weight_cart,w_cart);
#endif
}
/**
* @param acc the new acceleration in rad Robota frame of reference
* @return 1 if the new position is within the workspace boundaries
* and 0 otherwise (in that case the position is put on the boundaries)
*
***************************************************/
int Reaching::SetRobAcceleration(joint_vec_t& acc){
cart_vec_t& tmpc;
joint_vec_t& tmpa;
int ret =1;
float tmp;
#ifdef BODY_SCHEMA
joint_vec_t& min_angle, max_angle;
body->GetAnglesLowerBound(min_angle);
body->GetAnglesUpperBound(max_angle);
#endif
v4_copy(pos_angle,tmpa);
v4_add(v_angle,acc,v_angle);
v4_add(pos_angle,v_angle,pos_angle);
for (int i=0;i<4;i++){
tmp = min(max_angle[i],max(min_angle[i],pos_angle[i]));
if(tmp !=pos_angle[i]){
ret = 0;
pos_angle[i] = tmp;
}
}
if (!ret){
v4_sub(pos_angle,tmpa,v_angle);
}
v_copy(pos_cart,tmpc);
Angle2Cart(pos_angle,pos_cart);
v_sub(pos_cart,tmpc,v_cart);
return ret;
}
static void kalman_correct(kalman_t *kf, float p, float v)
{
/* K = P * HT * inv(H * P * HT + R) */
m_mlt(kf->H, kf->P, kf->T0);
mmtr_mlt(kf->T0, kf->H, kf->T1);
m_add(kf->T1, kf->R, kf->T0);
m_inverse(kf->T0, kf->T1);
mmtr_mlt(kf->P, kf->H, kf->T0);
m_mlt(kf->T0, kf->T1, kf->K);
/* x = x + K * (z - H * x) */
mv_mlt(kf->H, kf->x, kf->t0);
v_set_val(kf->z, 0, p);
v_set_val(kf->z, 1, v);
v_sub(kf->z, kf->t0, kf->t1);
mv_mlt(kf->K, kf->t1, kf->t0);
v_add(kf->x, kf->t0, kf->t1);
v_copy(kf->t1, kf->x);
/* P = (I - K * H) * P */
m_mlt(kf->K, kf->H, kf->T0);
m_sub(kf->I, kf->T0, kf->T1);
m_mlt(kf->T1, kf->P, kf->T0);
m_copy(kf->T0, kf->P);
}
VEC *pxinv_vec(PERM *px, const VEC *x, VEC *out)
#endif
{
unsigned int i, size;
if ( ! px || ! x )
error(E_NULL,"pxinv_vec");
if ( px->size > x->dim )
error(E_SIZES,"pxinv_vec");
/* if ( x == out )
error(E_INSITU,"pxinv_vec"); */
if ( ! out || out->dim < x->dim )
out = v_resize(out,x->dim);
size = px->size;
if ( size == 0 )
return v_copy(x,out);
if ( out != x )
{
for ( i=0; i<size; i++ )
if ( px->pe[i] >= size )
error(E_BOUNDS,"pxinv_vec");
else
out->ve[px->pe[i]] = x->ve[i];
}
else
{ /* in situ algorithm --- cheat's way out */
px_inv(px,px);
px_vec(px,x,out);
px_inv(px,px);
}
return out;
}
VEC *vm_mltadd(const VEC *v1, const VEC *v2, const MAT *A,
double alpha, VEC *out)
#endif
{
int /* i, */ j, m, n;
Real tmp, /* *A_e, */ *out_ve;
if ( ! v1 || ! v2 || ! A )
error(E_NULL,"vm_mltadd");
if ( v2 == out )
error(E_INSITU,"vm_mltadd");
if ( v1->dim != A->n || A->m != v2->dim )
error(E_SIZES,"vm_mltadd");
tracecatch(out = v_copy(v1,out),"vm_mltadd");
out_ve = out->ve; m = A->m; n = A->n;
for ( j = 0; j < m; j++ )
{
tmp = v2->ve[j]*alpha;
if ( tmp != 0.0 )
__mltadd__(out_ve,A->me[j],tmp,(int)n);
/**************************************************
A_e = A->me[j];
for ( i = 0; i < n; i++ )
out_ve[i] += A_e[i]*tmp;
**************************************************/
}
return out;
}
VEC *pxinv_vec(PERM *px, const VEC *x, VEC *out)
{
unsigned int i, size;
if ( ! px || ! x )
error(E_NULL,"pxinv_vec");
if ( px->size > x->dim )
error(E_SIZES,"pxinv_vec");
if ( ! out || out->dim < x->dim )
out = v_resize(out,x->dim);
size = px->size;
if ( size == 0 )
return v_copy(x,out);
if ( out != x )
{
for ( i=0; i<size; i++ )
if ( px->pe[i] >= size )
error(E_BOUNDS,"pxinv_vec");
else
out->ve[px->pe[i]] = x->ve[i];
}
else
{
px_inv(px,px);
px_vec(px,x,out);
px_inv(px,px);
}
return out;
}
//--------------------------------------------------------------------------
void Hqp_IpRedSpBKP::step(const Hqp_Program *qp, const VEC *z, const VEC *w,
const VEC *r1, const VEC *r2, const VEC *r3,
const VEC *r4, VEC *dx, VEC *dy, VEC *dz, VEC *dw)
{
VEC v;
assert((int)r1->dim == _n && (int)dx->dim == _n);
assert((int)r2->dim == _me && (int)dy->dim == _me);
assert((int)r3->dim == _m && (int)dz->dim == _m);
assert((int)r4->dim == _m && (int)dw->dim == _m);
// augment, copy, scale and permutate [r1;r2;r3] into _r12
// calculate, permutate and scale x
// augment r1
// temporary store (W^{-1}r_4 + ZW^{-1}r_3) in dz
v_part(_r12, 0, _n, &v);
v_slash(w, r4, dw);
v_star(_zw, r3, dz);
v_add(dw, dz, dz);
sp_mv_mlt(_CT, dz, &v);
v_sub(r1, &v, &v);
v_star(&v, _scale, &v);
v_copy(r2, v_part(_r12, _n, _me, &v));
px_vec(_J2QP, _r12, _r12);
spBKPsolve(_J, _pivot, _r12, _xy);
px_vec(_QP2J, _xy, _xy);
v_star(v_part(_xy, 0, _n, &v), _scale, dx);
v_copy(v_part(_xy, _n, _me, &v), dy);
sp_vm_mlt(_CT, dx, dw);
v_star(_zw, dw, dw);
v_sub(dz, dw, dz);
// calculate dw
sv_mlt(-1.0, r3, dw);
sp_mv_mltadd(dw, dx, qp->C, 1.0, dw);
// usage of _CT is numerically worse!
//sp_vm_mltadd(dw, dx, _CT, 1.0, dw);
}
void v_cross(vector result, vector v1, vector v2){
vector temp;
temp[0] = v1[1]*v2[2] - v1[2]*v2[1];
temp[1] = v1[2]*v2[0] - v1[0]*v2[2];
temp[2] = v1[0]*v2[1] - v1[1]*v2[0];
temp[3] = 0;
v_copy(result,temp);
}
Any _Data_copy (Data me) {
try {
if (me == NULL) return NULL;
autoData thee = (Data) _Thing_new (my classInfo);
my v_copy (thee.peek());
Thing_setName (thee.peek(), my name);
return thee.transfer();
} catch (MelderError) {
Melder_throw (me, ": not copied.");
}
}
Daata _Data_copy (Daata me) {
try {
if (! me) return nullptr;
autoDaata thee = static_cast <Daata> (Thing_newFromClass (my classInfo));
my v_copy (thee.peek());
Thing_setName (thee.peek(), my name);
return thee.transfer();
} catch (MelderError) {
Melder_throw (me, U": not copied.");
}
}
autoDaata _Data_copy (Daata me) {
try {
if (! me) return autoDaata();
autoDaata thee = Thing_newFromClass (my classInfo).static_cast_move <structDaata> ();
my v_copy (thee.peek());
Thing_setName (thee.peek(), my name);
return thee;
} catch (MelderError) {
Melder_throw (me, U": not copied.");
}
}
static int intersect(float fDst1, float fDst2,
const vec3_t a, const vec3_t b, vec3_t hit)
{
if ((fDst1 * fDst2) >= 0.0f)
return 0;
if (fDst1 == fDst2)
return 0;
//v_sub(hit, b, a);
//v_add(hit, a);
v_copy(hit, b);
v_scale(hit, (-fDst1 / (fDst2 - fDst1)));
return 1;
}
VEC *LUTsolve(const MAT *LU, PERM *pivot, const VEC *b, VEC *x)
#endif
{
if ( ! LU || ! b || ! pivot )
error(E_NULL,"LUTsolve");
if ( LU->m != LU->n || LU->n != b->dim )
error(E_SIZES,"LUTsolve");
x = v_copy(b,x);
UTsolve(LU,x,x,0.0); /* explicit diagonal */
LTsolve(LU,x,x,1.0); /* implicit diagonal = 1 */
pxinv_vec(pivot,x,x); /* x := P^T.tmp */
return (x);
}
VEC *spLUsolve(const SPMAT *A, PERM *pivot, const VEC *b, VEC *x)
#endif
{
int i, idx, len, lim;
Real sum, *x_ve;
SPROW *r;
row_elt *elt;
if ( ! A || ! b )
error(E_NULL,"spLUsolve");
if ( (pivot != PNULL && A->m != pivot->size) || A->m != b->dim )
error(E_SIZES,"spLUsolve");
if ( ! x || x->dim != A->n )
x = v_resize(x,A->n);
if ( pivot != PNULL )
x = px_vec(pivot,b,x);
else
x = v_copy(b,x);
x_ve = x->ve;
lim = min(A->m,A->n);
for ( i = 0; i < lim; i++ )
{
sum = x_ve[i];
r = &(A->row[i]);
len = r->len;
elt = r->elt;
for ( idx = 0; idx < len && elt->col < i; idx++, elt++ )
sum -= elt->val*x_ve[elt->col];
x_ve[i] = sum;
}
for ( i = lim-1; i >= 0; i-- )
{
sum = x_ve[i];
r = &(A->row[i]);
len = r->len;
elt = &(r->elt[len-1]);
for ( idx = len-1; idx >= 0 && elt->col > i; idx--, elt-- )
sum -= elt->val*x_ve[elt->col];
if ( idx < 0 || elt->col != i || elt->val == 0.0 )
error(E_SING,"spLUsolve");
x_ve[i] = sum/elt->val;
}
return x;
}
/* rot_vec -- apply Givens rotation to x's i & k components */
extern VEC *rot_vec(VEC *x, u_int i, u_int k, double c, double s, VEC *out)
{
Real temp;
if ( x==VNULL )
error(E_NULL,"rot_vec");
if ( i >= x->dim || k >= x->dim )
error(E_RANGE,"rot_vec");
out = v_copy(x,out);
/* temp = c*out->ve[i] + s*out->ve[k]; */
temp = c*v_entry(out,i) + s*v_entry(out,k);
/* out->ve[k] = -s*out->ve[i] + c*out->ve[k]; */
v_set_val(out,k,-s*v_entry(out,i)+c*v_entry(out,k));
/* out->ve[i] = temp; */
v_set_val(out,i,temp);
return (out);
}
VEC *vbd_mltadd(const VEC *x, const VEC *y, const BAND *bA,
double s, VEC *out)
#endif
{
int i, j;
if ( ! bA || ! x || ! y )
error(E_NULL,"vbd_mltadd");
if ( bA->mat->n != x->dim || y->dim != x->dim )
error(E_SIZES,"vbd_mltadd");
if ( ! out || out->dim != x->dim )
out = v_resize(out,x->dim);
out = v_copy(x,out);
for ( j = 0; j < x->dim; j++ )
for ( i = max(j-bA->ub,0); i <= j+bA->lb && i < x->dim; i++ )
out->ve[j] += s*bd_get_val(bA,i,j)*y->ve[i];
return out;
}
double
SpectralDifferenceAudioCurve::processDouble(const double *mag, int increment)
{
double result = 0.0;
const int hs1 = m_lastPerceivedBin + 1;
v_convert(m_tmpbuf, mag, hs1);
v_square(m_tmpbuf, hs1);
v_subtract(m_mag, m_tmpbuf, hs1);
v_abs(m_mag, hs1);
v_sqrt(m_mag, hs1);
for (int i = 0; i < hs1; ++i) {
result += m_mag[i];
}
v_copy(m_mag, m_tmpbuf, hs1);
return result;
}
int collide_box_line(const vec3_t mins, const vec3_t maxs,
const vec3_t a, const vec3_t b, vec3_t hit)
{
/* fast checks, both ends of line on same side of 6 planes */
if (b[0] < mins[0] && a[0] < mins[0])
return 0;
if (b[0] > maxs[0] && a[0] > maxs[0])
return 0;
if (b[1] < mins[1] && a[1] < mins[1])
return 0;
if (b[1] > maxs[1] && a[1] > maxs[1])
return 0;
if (b[2] < mins[2] && a[2] < mins[2])
return 0;
if (b[2] > maxs[2] && a[2] > maxs[2])
return 0;
/* start inside box */
if (a[0] > mins[0] && a[0] < maxs[0] &&
a[1] > mins[1] && a[1] < maxs[1] &&
a[2] > mins[2] && a[2] < maxs[2]) {
v_copy(hit, a);
return 1;
}
if ((intersect(a[0] - mins[0], b[0] - mins[0], a, b, hit)
&& in_box(hit, mins, maxs, 1))
|| (intersect(a[1] - mins[1], b[1] - mins[1], a, b, hit)
&& in_box(hit, mins, maxs, 2))
|| (intersect(a[2] - mins[2], b[2] - mins[2], a, b, hit)
&& in_box(hit, mins, maxs, 3))
|| (intersect(a[0] - maxs[0], b[0] - maxs[0], a, b, hit)
&& in_box(hit, mins, maxs, 1))
|| (intersect(a[1] - maxs[1], b[1] - maxs[1], a, b, hit)
&& in_box(hit, mins, maxs, 2))
|| (intersect(a[2] - maxs[2], b[2] - maxs[2], a, b, hit)
&& in_box(hit, mins, maxs, 3)))
return 1;
return 0;
}
int KChainOrientationBodySchema::InverseKinematics(const cart_vec_t& pos,joint_vec_t& angles){
float f;
int it=0;
CQuat_t q;
v_copy(pos.GetArray()+3,q);
q[3] = sqrtf(1-v_squ_length(q));
f=InverseKinematicsCCD(pos.GetArray(),q);
if(f>tol){
cout<<"can't find inverse kinematics"<<endl;
return 0;
}
KinematicChain::GetAngles(angles.GetArray());
// if(!AnglesInRange(angles)){
// Matrix jac,pjac,mat(joint_angle_dim,joint_angle_dim);
// joint_vec_t dangle,middle;
// middle = (min_angle+max_angle)*0.5;
// do{ // gets back to allowable range
// Jacobian(angles,jac);
// jac.Inverse(pjac); //pseudo-inverse
// if(!Matrix::IsInverseOk())
// {cout<<"bad matrix inversion in InverseKinematics"<<endl;}
// mat.Identity();
// mat -= pjac*jac;
// dangle = mat*(middle-angles)*0.01;
// angles += dangle;
// // cout<<"ik "<<angles<<endl;
// //mat.Print();
// }while(!AnglesInRange(angles) && it++ <100);
// if(it<=100){
// cout<<"can't bring target configuration in range"<<endl;
// }
// else{
// cout<<"in range"<<endl;
// }
// f=InverseKinPosCCD(pos.GetArray());
// }
return f<=tol;
}
/* hhtrvec -- apply Householder transformation to vector -- may be in-situ */
VEC *hhtrvec(VEC *hh,double beta,unsigned int i0,VEC *in,VEC *out)
/* VEC *hh,*in,*out; hh = Householder vector */
{
Real scale;
/* unsigned int i; */
if ( hh==(VEC *)NULL || in==(VEC *)NULL )
error(E_NULL,"hhtrvec");
if ( in->dim != hh->dim )
error(E_SIZES,"hhtrvec");
if ( i0 > in->dim )
error(E_BOUNDS,"hhtrvec");
scale = beta*_in_prod(hh,in,i0);
out = v_copy(in,out);
__mltadd__(&(out->ve[i0]),&(hh->ve[i0]),-scale,(int)(in->dim-i0));
/************************************************************
for ( i=i0; i<in->dim; i++ )
out->ve[i] = in->ve[i] - scale*hh->ve[i];
************************************************************/
return (out);
}
int collide_box_line(const vec3_t mins, const vec3_t maxs,
const vec3_t a, const vec3_t b, vec3_t hit)
{
if (b[0] < mins[0] && a[0] < mins[0])
return 0;
if (b[0] > maxs[0] && a[0] > maxs[0])
return 0;
if (b[1] < mins[1] && a[1] < mins[1])
return 0;
if (b[1] > maxs[1] && a[1] > maxs[1])
return 0;
if (b[2] < mins[2] && a[2] < mins[2])
return 0;
if (b[2] > maxs[2] && a[2] > maxs[2])
return 0;
if (a[0] > mins[0] && a[0] < maxs[0] &&
a[1] > mins[1] && a[1] < maxs[1] && a[2] > mins[2]
&& a[2] < maxs[2]) {
v_copy(hit, a);
return 1;
}
if ((intersect(a[0] - mins[0], b[0] - mins[0], a, b, hit)
&& in_box(hit, mins, maxs, 1))
|| (intersect(a[1] - mins[1], b[1] - mins[1], a, b, hit)
&& in_box(hit, mins, maxs, 2))
|| (intersect(a[2] - mins[2], b[2] - mins[2], a, b, hit)
&& in_box(hit, mins, maxs, 3))
|| (intersect(a[0] - maxs[0], b[0] - maxs[0], a, b, hit)
&& in_box(hit, mins, maxs, 1))
|| (intersect(a[1] - maxs[1], b[1] - maxs[1], a, b, hit)
&& in_box(hit, mins, maxs, 2))
|| (intersect(a[2] - maxs[2], b[2] - maxs[2], a, b, hit)
&& in_box(hit, mins, maxs, 3)))
return 1;
return 0;
}
VEC *mv_mltadd(const VEC *v1, const VEC *v2, const MAT *A,
double alpha, VEC *out)
#endif
{
/* register int j; */
int i, m, n;
Real *v2_ve, *out_ve;
if ( ! v1 || ! v2 || ! A )
error(E_NULL,"mv_mltadd");
if ( out == v2 )
error(E_INSITU,"mv_mltadd");
if ( v1->dim != A->m || v2->dim != A->n )
error(E_SIZES,"mv_mltadd");
tracecatch(out = v_copy(v1,out),"mv_mltadd");
v2_ve = v2->ve; out_ve = out->ve;
m = A->m; n = A->n;
if ( alpha == 0.0 )
return out;
for ( i = 0; i < m; i++ )
{
out_ve[i] += alpha*__ip__(A->me[i],v2_ve,(int)n);
/**************************************************
A_e = A->me[i];
sum = 0.0;
for ( j = 0; j < n; j++ )
sum += A_e[j]*v2_ve[j];
out_ve[i] = v1->ve[i] + alpha*sum;
**************************************************/
}
return out;
}
double rk4(double t, VEC *x, double h, VEC *Torq)
{
VEC *v1=VNULL, *v2=VNULL, *v3=VNULL, *v4=VNULL;
VEC *temp=VNULL;
double step_size=0.5*h;
if ( x == VNULL )
error(E_NULL,"rk4");
v1 = v_get(x->dim);
v2 = v_get(x->dim);
v3 = v_get(x->dim);
v4 = v_get(x->dim);
temp = v_get(x->dim);
Sy_m(x, Torq, v1);
v_mltadd(x,v1,step_size,temp);
Sy_m(temp, Torq, v2);
v_mltadd(x,v2,step_size,temp);
Sy_m(temp, Torq, v3);
step_size = h;
v_mltadd(x, v3, step_size, temp);
Sy_m(temp, Torq, v4);
temp = v_copy(v1,temp);
v_mltadd(temp,v2,2.0,temp);
v_mltadd(temp,v3,2.0,temp);
v_add(temp,v4,temp);
v_mltadd(x,temp,(h/6.0),x);
return t+h;
V_FREE(v1);
V_FREE(v2);
V_FREE(v3);
V_FREE(v4);
V_FREE(temp);
}
VEC *_Qsolve(const MAT *QR, const VEC *diag, const VEC *b,
VEC *x, VEC *tmp)
#endif
{
unsigned int dynamic;
int k, limit;
Real beta, r_ii, tmp_val;
limit = min(QR->m,QR->n);
dynamic = FALSE;
if ( ! QR || ! diag || ! b )
error(E_NULL,"_Qsolve");
if ( diag->dim < limit || b->dim != QR->m )
error(E_SIZES,"_Qsolve");
x = v_resize(x,QR->m);
if ( tmp == VNULL )
dynamic = TRUE;
tmp = v_resize(tmp,QR->m);
/* apply H/holder transforms in normal order */
x = v_copy(b,x);
for ( k = 0 ; k < limit ; k++ )
{
get_col(QR,k,tmp);
r_ii = fabs(tmp->ve[k]);
tmp->ve[k] = diag->ve[k];
tmp_val = (r_ii*fabs(diag->ve[k]));
beta = ( tmp_val == 0.0 ) ? 0.0 : 1.0/tmp_val;
/* hhtrvec(tmp,beta->ve[k],k,x,x); */
hhtrvec(tmp,beta,k,x,x);
}
if ( dynamic )
V_FREE(tmp);
return (x);
}
int main()
{
std::vector<double> v(4);
for (int i=0; i<v.size(); ++i)
v[i] = i+1;
omp_set_nested(1);
#pragma omp parallel num_threads(2)
{
#pragma omp master
{
std::cout << "Parallel region with " << omp_get_num_threads() << " threads" << std::endl;
int iter = 0;
while (true)
{
#pragma omp critical
std::cout << "Iter = " << iter << ", Main thread: " << omp_get_thread_num() << std::endl;
if (iter % 20 == 0)
{
#pragma omp taskwait
std::vector<double> v_copy(v);
#pragma omp task shared(v_copy), firstprivate(iter)
stats(v_copy, iter);
}
++iter;
std::this_thread::sleep_for(std::chrono::milliseconds(200));
}
}
}
return 0;
}
/* sv_mlt -- scalar-vector multiply -- may be in-situ */
VEC *sv_mlt(double scalar,VEC *vector,VEC *out)
{
/* u_int dim, i; */
/* Real *out_ve, *vec_ve; */
if ( vector==(VEC *)NULL )
error(E_NULL,"sv_mlt");
if ( out==(VEC *)NULL || out->dim != vector->dim )
out = v_resize(out,vector->dim);
if ( scalar == 0.0 )
return v_zero(out);
if ( scalar == 1.0 )
return v_copy(vector,out);
__smlt__(vector->ve,(double)scalar,out->ve,(int)(vector->dim));
/**************************************************
dim = vector->dim;
out_ve = out->ve; vec_ve = vector->ve;
for ( i=0; i<dim; i++ )
out->ve[i] = scalar*vector->ve[i];
(*out_ve++) = scalar*(*vec_ve++);
**************************************************/
return (out);
}
VEC *iter_mgcr(ITER *ip)
#endif
{
STATIC VEC *As=VNULL, *beta=VNULL, *alpha=VNULL, *z=VNULL;
STATIC MAT *N=MNULL, *H=MNULL;
VEC *rr, v, s; /* additional pointer and structures */
Real nres; /* norm of a residual */
Real dd; /* coefficient d_i */
int i,j;
int done; /* if TRUE then stop the iterative process */
int dim; /* dimension of the problem */
/* ip cannot be NULL */
if (ip == INULL) error(E_NULL,"mgcr");
/* Ax, b and stopping criterion must be given */
if (! ip->Ax || ! ip->b || ! ip->stop_crit)
error(E_NULL,"mgcr");
/* at least one direction vector must exist */
if ( ip->k <= 0) error(E_BOUNDS,"mgcr");
/* if the vector x is given then b and x must have the same dimension */
if ( ip->x && ip->x->dim != ip->b->dim)
error(E_SIZES,"mgcr");
if (ip->eps <= 0.0) ip->eps = MACHEPS;
dim = ip->b->dim;
As = v_resize(As,dim);
alpha = v_resize(alpha,ip->k);
beta = v_resize(beta,ip->k);
MEM_STAT_REG(As,TYPE_VEC);
MEM_STAT_REG(alpha,TYPE_VEC);
MEM_STAT_REG(beta,TYPE_VEC);
H = m_resize(H,ip->k,ip->k);
N = m_resize(N,ip->k,dim);
MEM_STAT_REG(H,TYPE_MAT);
MEM_STAT_REG(N,TYPE_MAT);
/* if a preconditioner is defined */
if (ip->Bx) {
z = v_resize(z,dim);
MEM_STAT_REG(z,TYPE_VEC);
}
/* if x is NULL then it is assumed that x has
entries with value zero */
if ( ! ip->x ) {
ip->x = v_get(ip->b->dim);
ip->shared_x = FALSE;
}
/* v and s are additional pointers to rows of N */
/* they must have the same dimension as rows of N */
v.dim = v.max_dim = s.dim = s.max_dim = dim;
done = FALSE;
for (ip->steps = 0; ip->steps < ip->limit; ) {
(*ip->Ax)(ip->A_par,ip->x,As); /* As = A*x */
v_sub(ip->b,As,As); /* As = b - A*x */
rr = As; /* rr is an additional pointer */
/* if a preconditioner is defined */
if (ip->Bx) {
(*ip->Bx)(ip->B_par,As,z); /* z = B*(b-A*x) */
rr = z;
}
/* norm of the residual */
nres = v_norm2(rr);
dd = nres; /* dd = ||r_i|| */
/* check if the norm of the residual is zero */
if (ip->steps == 0) {
/* information for a user */
if (ip->info) (*ip->info)(ip,nres,As,rr);
ip->init_res = fabs(nres);
}
if (nres == 0.0) {
/* iterative process is finished */
done = TRUE;
break;
}
/* save this residual in the first row of N */
v.ve = N->me[0];
v_copy(rr,&v);
for (i = 0; i < ip->k && ip->steps < ip->limit; i++) {
ip->steps++;
v.ve = N->me[i]; /* pointer to a row of N (=s_i) */
/* note that we must use here &v, not v */
(*ip->Ax)(ip->A_par,&v,As);
rr = As; /* As = A*s_i */
if (ip->Bx) {
(*ip->Bx)(ip->B_par,As,z); /* z = B*A*s_i */
rr = z;
}
if (i < ip->k - 1) {
s.ve = N->me[i+1]; /* pointer to a row of N (=s_{i+1}) */
v_copy(rr,&s); /* s_{i+1} = B*A*s_i */
for (j = 0; j <= i-1; j++) {
v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */
/* beta->ve[j] = in_prod(&v,rr); */ /* beta_{j,i} */
/* modified Gram-Schmidt algorithm */
beta->ve[j] = in_prod(&v,&s); /* beta_{j,i} */
/* s_{i+1} -= beta_{j,i}*s_{j+1} */
v_mltadd(&s,&v,- beta->ve[j],&s);
}
/* beta_{i,i} = ||s_{i+1}||_2 */
beta->ve[i] = nres = v_norm2(&s);
if ( nres <= MACHEPS*ip->init_res) {
/* s_{i+1} == 0 */
i--;
done = TRUE;
break;
}
sv_mlt(1.0/nres,&s,&s); /* normalize s_{i+1} */
v.ve = N->me[0];
alpha->ve[i] = in_prod(&v,&s); /* alpha_i = (s_0 , s_{i+1}) */
}
else {
for (j = 0; j <= i-1; j++) {
v.ve = N->me[j+1]; /* pointer to a row of N (=s_{j+1}) */
beta->ve[j] = in_prod(&v,rr); /* beta_{j,i} */
}
nres = in_prod(rr,rr); /* rr = B*A*s_{k-1} */
for (j = 0; j <= i-1; j++)
nres -= beta->ve[j]*beta->ve[j];
if (sqrt(fabs(nres)) <= MACHEPS*ip->init_res) {
/* s_k is zero */
i--;
done = TRUE;
break;
}
if (nres < 0.0) { /* do restart */
i--;
ip->steps--;
break;
}
beta->ve[i] = sqrt(nres); /* beta_{k-1,k-1} */
v.ve = N->me[0];
alpha->ve[i] = in_prod(&v,rr);
for (j = 0; j <= i-1; j++)
alpha->ve[i] -= beta->ve[j]*alpha->ve[j];
alpha->ve[i] /= beta->ve[i]; /* alpha_{k-1} */
}
set_col(H,i,beta);
/* other method of computing dd */
/* if (fabs((double)alpha->ve[i]) > dd) {
nres = - dd*dd + alpha->ve[i]*alpha->ve[i];
nres = sqrt((double) nres);
if (ip->info) (*ip->info)(ip,-nres,VNULL,VNULL);
break;
} */
/* to avoid overflow/underflow in computing dd */
/* dd *= cos(asin((double)(alpha->ve[i]/dd))); */
nres = alpha->ve[i]/dd;
if (fabs(nres-1.0) <= MACHEPS*ip->init_res)
dd = 0.0;
else {
nres = 1.0 - nres*nres;
if (nres < 0.0) {
nres = sqrt((double) -nres);
if (ip->info) (*ip->info)(ip,-dd*nres,VNULL,VNULL);
break;
}
dd *= sqrt((double) nres);
}
if (ip->info) (*ip->info)(ip,dd,VNULL,VNULL);
if ( ip->stop_crit(ip,dd,VNULL,VNULL) ) {
/* stopping criterion is satisfied */
done = TRUE;
break;
}
} /* end of for */
if (i >= ip->k) i = ip->k - 1;
/* use (i+1) by (i+1) submatrix of H */
H = m_resize(H,i+1,i+1);
alpha = v_resize(alpha,i+1);
Usolve(H,alpha,alpha,0.0); /* c_i is saved in alpha */
for (j = 0; j <= i; j++) {
v.ve = N->me[j];
v_mltadd(ip->x,&v,alpha->ve[j],ip->x);
}
if (done) break; /* stop the iterative process */
alpha = v_resize(alpha,ip->k);
H = m_resize(H,ip->k,ip->k);
} /* end of while */
#ifdef THREADSAFE
V_FREE(As);
V_FREE(beta);
V_FREE(alpha);
V_FREE(z);
M_FREE(N);
M_FREE(H);
#endif
return ip->x; /* return the solution */
}
