/**
* @function connectGoal
*/
bool JG_RRT::connectGoal()
{
Eigen::MatrixXd J(3,7);
Eigen::MatrixXd Jinv(7,3);
Eigen::MatrixXd delta_x;
Eigen::MatrixXd delta_q;
Eigen::MatrixXd q_new;
JG_RRT::StepResult result;
double dist_new;
double dist_old;
result = STEP_PROGRESS;
dist_new = 0;
dist_old = DBL_MAX;
int i = 0;
while( result == STEP_PROGRESS && (dist_new < dist_old) )
{ //-- Get the closest node to the goal ( workspace metric )
int NNidx = pop_Ranking();
if( NNidx == -1 )
{
break;
}
Eigen::VectorXd q_closest = configVector[NNidx];
dist_old = wsDistance( q_closest );
//-- Get the Jacobian
robinaLeftArm_j( q_closest, TWBase, Tee, J );
//-- Get the pseudo-inverse (easy way)
pseudoInv( 3, 7, J, Jinv );
//-- Get the workspace
delta_x = wsDiff( q_closest );
delta_q = Jinv*delta_x;
//-- Scale -- with this 2 actually result will always be in PROGRESS, if not COLLISION
double scal = 2*stepSize/delta_q.norm();
delta_q = delta_q*scal;
//-- Get q_new
q_new = q_closest + delta_q;
//-- Attempt a new step towards J
result = tryStep( q_new, NNidx );
i++;
//-- Check
if( result == STEP_PROGRESS )
{ dist_new = wsDistance( configVector[configVector.size() - 1] );
if( dist_new < distanceThresh )
{
result = STEP_REACHED;
break;
}
}
}
return( result == STEP_REACHED );
}
int IsoparametricTransformation::TransformBack(const Vector &pt,
IntegrationPoint &ip)
{
const int max_iter = 16;
const double ref_tol = 1e-15;
const double phys_tol = 1e-15*pt.Normlinf();
const int dim = FElem->GetDim();
const int sdim = PointMat.Height();
const int geom = FElem->GetGeomType();
IntegrationPoint xip, prev_xip;
double xd[3], yd[3], dxd[3], Jid[9];
Vector x(xd, dim), y(yd, sdim), dx(dxd, dim);
DenseMatrix Jinv(Jid, dim, sdim);
bool hit_bdr = false, prev_hit_bdr;
// Use the center of the element as initial guess
xip = Geometries.GetCenter(geom);
xip.Get(xd, dim); // xip -> x
for (int it = 0; it < max_iter; it++)
{
// Newton iteration: x := x + J(x)^{-1} [pt-F(x)]
// or when dim != sdim: x := x + [J^t.J]^{-1}.J^t [pt-F(x)]
Transform(xip, y);
subtract(pt, y, y); // y = pt-y
if (y.Normlinf() < phys_tol) { ip = xip; return 0; }
SetIntPoint(&xip);
CalcInverse(Jacobian(), Jinv);
Jinv.Mult(y, dx);
x += dx;
prev_xip = xip;
prev_hit_bdr = hit_bdr;
xip.Set(xd, dim); // x -> xip
// If xip is ouside project it on the boundary on the line segment
// between prev_xip and xip
hit_bdr = !Geometry::ProjectPoint(geom, prev_xip, xip);
if (dx.Normlinf() < ref_tol) { ip = xip; return 0; }
if (hit_bdr)
{
xip.Get(xd, dim); // xip -> x
if (prev_hit_bdr)
{
prev_xip.Get(dxd, dim); // prev_xip -> dx
subtract(x, dx, dx); // dx = xip - prev_xip
if (dx.Normlinf() < ref_tol) { return 1; }
}
}
}
ip = xip;
return 2;
}
virtual void compute_solution(const PhysData &inner_cell_data, const RealVectorNDIM &boundary_face_normal, RealVectorNEQS &boundary_face_solution)
{
enum {ENTR=0,OMEGA=1,APLUS=2,AMIN=3};
RealMatrix J((int)NDIM,(int)NDIM);
RealMatrix Jinv((int)NDIM,(int)NDIM);
Real detJ;
RealMatrix nodes;
inner_cell->get().space->support().geometry_space().allocate_coordinates(nodes);
inner_cell->get().space->support().geometry_space().put_coordinates(nodes,inner_cell->get().idx);
inner_cell->get().space->support().element_type().compute_jacobian(inner_cell->get().sf->flx_pts().row(cell_flx_pt),nodes,J);
Jinv = J.inverse();
detJ = J.determinant();
// Real Jxi = Jinv.row(KSI).norm();
// Real Jeta = Jinv.row(ETA).norm();
// std::cout << "Jxi = " << Jxi << " Jeta = " << Jeta << std::endl;
outward_normal = flx_pt_plane_jacobian_normal->get().plane_unit_normal[cell_flx_pt] * flx_pt_plane_jacobian_normal->get().sf->flx_pt_sign(cell_flx_pt);
const RealRowVector& n = outward_normal;
RealRowVector s((int)NDIM); s << n[YY], -n[XX];
// std::cout << "n = " << n << std::endl;
// std::cout << "s = " << s << std::endl;
// RealVector U0(2);
// U0[XX] = options().value< std::vector<Real> >("U0")[XX];
// U0[YY] = options().value< std::vector<Real> >("U0")[YY];
// Real u0n = U0.dot(n);
// Real u0s = U0.dot(s);
RealRowVector mapped_n = n * detJ * Jinv;
RealRowVector mapped_s = s * detJ * Jinv;
// std::cout << "mapped_n = " << mapped_n << std::endl;
// std::cout << "mapped_s = " << mapped_s << std::endl;
// if ( std::abs(outward_normal[XX]-1.) > 1e-10)
// throw common::BadValue(FromHere(), "outward_normal incorrect");
for (Uint f=0; f<inner_cell->get().sf->nb_flx_pts(); ++f)
{
cons_to_char(cons_flx_pt_solution[f],outward_normal,char_flx_pt_solution[f]);
}
// Extrapolation BC
char_bdry_solution[ENTR ] = char_flx_pt_solution[cell_flx_pt][ENTR ];
char_bdry_solution[OMEGA] = char_flx_pt_solution[cell_flx_pt][OMEGA];
char_bdry_solution[APLUS] = char_flx_pt_solution[cell_flx_pt][APLUS];
char_bdry_solution[AMIN ] = char_flx_pt_solution[cell_flx_pt][AMIN ];
sf_deriv_xi.resize(inner_cell->get().sf->nb_flx_pts());
sf_deriv_eta.resize(inner_cell->get().sf->nb_flx_pts());
sf_deriv_n.resize(inner_cell->get().sf->nb_flx_pts());
sf_deriv_s.resize(inner_cell->get().sf->nb_flx_pts());
inner_cell->get().sf->compute_flux_derivative(KSI,inner_cell->get().sf->flx_pts().row(cell_flx_pt),sf_deriv_xi);
inner_cell->get().sf->compute_flux_derivative(ETA,inner_cell->get().sf->flx_pts().row(cell_flx_pt),sf_deriv_eta);
for (Uint j=0; j<inner_cell->get().sf->nb_flx_pts(); ++j)
{
sf_deriv_n[j] = mapped_n[KSI] * sf_deriv_xi[j] + mapped_n[ETA] * sf_deriv_eta[j];
sf_deriv_s[j] = mapped_s[KSI] * sf_deriv_xi[j] + mapped_s[ETA] * sf_deriv_eta[j];
}
std::vector<Real> omega(inner_cell->get().sf->nb_flx_pts());
Real dAmindn = 0.;
Real dAminds = 0.;
Real dAplusdn = 0.;
Real dAplusds = 0.;
Real dOmegadn = 0.;
Real dOmegads = 0.;
Real domegadn = 0.;
Real domegads = 0.;
for (Uint j=0; j<inner_cell->get().sf->nb_flx_pts(); ++j)
{
dOmegadn += char_flx_pt_solution[j][OMEGA] * sf_deriv_n[j];
dOmegads += char_flx_pt_solution[j][OMEGA] * sf_deriv_s[j];
dAmindn += char_flx_pt_solution[j][AMIN ] * sf_deriv_n[j];
dAminds += char_flx_pt_solution[j][AMIN ] * sf_deriv_s[j];
dAplusdn += char_flx_pt_solution[j][APLUS] * sf_deriv_n[j];
dAplusds += char_flx_pt_solution[j][APLUS] * sf_deriv_s[j];
omega[j] = 0.5*(char_flx_pt_solution[j][APLUS] - char_flx_pt_solution[j][AMIN]);
domegadn += omega[j] * sf_deriv_n[j];
domegads += omega[j] * sf_deriv_s[j];
}
// dA/dn + dOmega/ds = 0
// dAplus/dn - dAmin/dn = - 2 dOmega/ds
// 0 < (Aplus-Amin)/(Omega) < 1 --> tangent flow dominates
// (Aplus-Amin)/(Omega) > 1 --> normal flow dominates
// std::cout << "gradx = " << sf_deriv_xi.transpose() << std::endl;
// std::cout << "dAmindxi_internal = " << dAmindxi_internal << std::endl;
// Try 1
// dAmin/dt + (u0+c0)dAmin/dn + c0 dAmin/ds - c0 dOmega/ds = 0
// dAmindn = 1./(m_u0n-m_c0) * ( (m_u0n+m_c0)*dAmindn - 2.*m_c0*dOmegads );
// Try 2! dAmin/dt + u0n dOmega/ds + u0s dAmin/dn = 0
// dAmindn = dOmegads;
// dAmindn = -dAplusdn --> dAmindn = dOmegads
// dAmindn/dAplusdn = dAmindn/dOmegads
// BC divzero
// dAmindn = dAplusdn + 2*dOmegads;
// BC dAdt + u0n dAdn = 0 boils to same as divzero when u0s=0
// dAmindn = 1./m_c0*(u0s*dAminds+u0s*dAplusds+m_c0*dAplusdn+2*m_c0*dOmegads);
// Aplus*Amin > 0 --> pulse
// Aplus*Amin < 0 --> vortex
// Aplus + Amin = 0 --> vortex
// Aplus - Amin = 0 --> pulse
// Aplus+Amin =
// Real ratio = ( std::abs(dAmindn) < 1e-12 ? 0. : dAmindn/dOmegads);
// static Real i = 0.;
// static Real avg_ratio = ratio;
// static Real max_ratio = ratio;
// static Real min_ratio = ratio;
// if (ratio != 0 && std::abs(ratio) < 2)
// {
// max_ratio = std::max(max_ratio,ratio);
// min_ratio = std::min(min_ratio,ratio);
// avg_ratio = i/(i+1.) * avg_ratio + 1./(i+1.) * ratio;
// ++i;
//// std::cout << "ratio = " << ratio << "\t i = " << i << "\t avg = " << avg_ratio << " \t max = " << max_ratio << " \t min = " << min_ratio << std::endl;
// }
// vortex: ratio = 1. alpha = 0
// pulse: ratio = 0.5 alpha = 1
// Real alpha = std::max(0., std::min(1., 2.* (1.-std::abs(ratio)) ));
// std::cout << "alpha = " << alpha << std::endl;
Real alpha = options().value<Real>("alpha");
dAmindn = (1.-alpha)*dOmegads + alpha*dAmindn;
// dAmindn = dAmindn + 2.*dAmindn/dAplusdn*dOmegads;
// dAmindn = 0.;//dOmegads;
// Hedstrom: dAmin/dt = 0
// --> (u0n - c0) dAmin/dn + u0s dAmin/ds + c0 dOmega/ds = 0
// dAmindn = 1./(m_u0n-m_c0)*(-m_u0s*dAminds - m_c0*dOmegads);
// dAmin/dt + u0n dAmin/dn + u0s dAmin/ds = 0
// dAmindn = 1./(m_u0n+m_c0)*(m_u0n*dAmindn - m_c0*dOmegads);
// Try 3
// dAmindn = 2.*dOmegads + dAplusdn;
// replace = dAmindxi_internal + 2.*dOmegadeta;
// replace = -dAmindxi_internal;
// replace = -dAplusdxi;
// replace = dAmindxi_internal;
// replace = 2./(m_u0n-m_c0) * ( m_u0n/2.*dAmindxi_internal -m_c0/2.*dAplusdxi -m_c0*dOmegadeta );
// // Also modify equation for omega:
// dAplusdn = -dAmindn;
// Real A = 0.5*(char_flx_pt_solution[cell_flx_pt][APLUS] + char_flx_pt_solution[cell_flx_pt][AMIN ]);
// dAplusdn = -dAmindn;
char_bdry_solution[AMIN ] = dAmindn;
// char_bdry_solution[APLUS] = dAplusdn;
for (Uint j=0; j<inner_cell->get().sf->nb_flx_pts(); ++j)
{
if (j!=cell_flx_pt)
{
char_bdry_solution[AMIN ] -= char_flx_pt_solution[j][AMIN ] * sf_deriv_n[j];
// char_bdry_solution[APLUS] -= char_flx_pt_solution[j][APLUS] * sf_deriv_xi[j];
}
}
char_bdry_solution[AMIN ] /= sf_deriv_n[cell_flx_pt];
// Real A = 0.5*(char_flx_pt_solution[cell_flx_pt][APLUS] + char_flx_pt_solution[cell_flx_pt][AMIN]);
// char_bdry_solution[APLUS] = 2.*A - char_bdry_solution[AMIN];
// char_bdry_solution[APLUS] /= sf_deriv_xi[cell_flx_pt];
//// std::cout << char_bdry_solution[AMIN] << std::endl;
// compute_optimization(char_bdry_solution[OMEGA],char_bdry_solution[APLUS],char_bdry_solution[AMIN]);
// char_bdry_solution[APLUS] = char_flx_pt_solution[cell_flx_pt][APLUS];
// compute_optimization_2(char_bdry_solution[OMEGA],char_bdry_solution[AMIN]);
char_to_cons(char_bdry_solution,outward_normal,boundary_face_solution);
}
void
SSPquad::GetStab(void)
// this function computes the stabilization stiffness matrix for the element
{
Vector g1(SSPQ_NUM_DIM);
Vector g2(SSPQ_NUM_DIM);
Matrix I(SSPQ_NUM_DIM,SSPQ_NUM_DIM);
Matrix FCF(SSPQ_NUM_DIM,SSPQ_NUM_DIM);
Matrix Jmat(SSPQ_NUM_DIM,SSPQ_NUM_DIM);
Matrix Jinv(SSPQ_NUM_DIM,SSPQ_NUM_DIM);
Matrix dNloc(SSPQ_NUM_NODE,SSPQ_NUM_DIM);
Matrix dN(SSPQ_NUM_NODE,SSPQ_NUM_DIM);
Matrix Mben(2,SSPQ_NUM_DOF);
double Hss;
double Hst;
double Htt;
// shape function derivatives (local crd) at center
dNloc(0,0) = -0.25;
dNloc(1,0) = 0.25;
dNloc(2,0) = 0.25;
dNloc(3,0) = -0.25;
dNloc(0,1) = -0.25;
dNloc(1,1) = -0.25;
dNloc(2,1) = 0.25;
dNloc(3,1) = 0.25;
// jacobian matrix
Jmat = mNodeCrd*dNloc;
// inverse of the jacobian matrix
Jmat.Invert(Jinv);
// shape function derivatives (global crd)
dN = dNloc*Jinv;
// define hourglass stabilization vector gamma = 0.25*(h - (h^x)*bx - (h^y)*by);
double hx = mNodeCrd(0,0) - mNodeCrd(0,1) + mNodeCrd(0,2) - mNodeCrd(0,3);
double hy = mNodeCrd(1,0) - mNodeCrd(1,1) + mNodeCrd(1,2) - mNodeCrd(1,3);
double gamma[4];
gamma[0] = 0.25*( 1.0 - hx*dN(0,0) - hy*dN(0,1));
gamma[1] = 0.25*(-1.0 - hx*dN(1,0) - hy*dN(1,1));
gamma[2] = 0.25*( 1.0 - hx*dN(2,0) - hy*dN(2,1));
gamma[3] = 0.25*(-1.0 - hx*dN(3,0) - hy*dN(3,1));
// define mapping matrices
Mmem.Zero();
Mben.Zero();
for (int i = 0; i < 4; i++) {
Mmem(0,2*i) = dN(i,0);
Mmem(1,2*i+1) = dN(i,1);
Mmem(2,2*i) = dN(i,1);
Mmem(2,2*i+1) = dN(i,0);
Mben(0,2*i) = gamma[i];
Mben(1,2*i+1) = gamma[i];
}
// base vectors
g1(0) = Jmat(0,0);
g1(1) = Jmat(1,0);
g2(0) = Jmat(0,1);
g2(1) = Jmat(1,1);
// normalize base vectors
g1.Normalize();
g2.Normalize();
// compute second moment of area tensor
double fourThree = 4.0/3.0;
I = fourThree*mThickness*J0*(DyadicProd(g1,g1) + DyadicProd(g2,g2));
// stabilization terms
Hss = (I(0,0)*Jinv(1,0)*Jinv(1,0) + I(0,1)*Jinv(0,0)*Jinv(1,0) + I(1,1)*Jinv(0,0)*Jinv(0,0))*0.25;
Htt = (I(0,0)*Jinv(1,1)*Jinv(1,1) + I(0,1)*Jinv(0,1)*Jinv(1,1) + I(1,1)*Jinv(0,1)*Jinv(0,1))*0.25;
Hst = (I(0,0)*Jinv(1,1)*Jinv(1,0) + I(0,1)*(Jinv(1,0)*Jinv(0,1) + Jinv(1,1)*Jinv(0,0)) + I(1,1)*Jinv(0,1)*Jinv(0,0))*0.25;
// get material tangent
const Matrix &CmatI = theMaterial->getInitialTangent();
// compute stabilization matrix
FCF(0,0) = (CmatI(0,0) - (CmatI(0,1) + CmatI(1,0)) + CmatI(1,1))*Hss;
FCF(0,1) = (CmatI(0,1) - (CmatI(0,0) + CmatI(1,1)) + CmatI(1,0))*Hst;
FCF(1,0) = (CmatI(1,0) - (CmatI(0,0) + CmatI(1,1)) + CmatI(0,1))*Hst;
FCF(1,1) = (CmatI(1,1) - (CmatI(0,1) + CmatI(1,0)) + CmatI(0,0))*Htt;
// compute stiffness matrix for stabilization terms
Kstab.Zero();
Kstab.addMatrixTripleProduct(1.0, Mben, FCF, 1.0);
return;
}
double IK::solve_ik()
{
int i, j;
double current_max_condnum = -1.0;
copy_jacobian();
if(n_all_const > 0)
{
////
// check rank when HIGH_IF_POSSIBLE constraints have high priority
int n_high_const;
fMat Jhigh;
fMat wJhigh;
fVec fb_high;
fVec weight_high;
int* is_high_const = 0;
double cond_number = 1.0;
// cerr << "---" << endl;
// cerr << n_const[HIGH_PRIORITY] << " " << n_const[HIGH_IF_POSSIBLE] << " " << n_const[LOW_PRIORITY] << endl;
if(n_const[HIGH_IF_POSSIBLE] > 0)
{
is_high_const = new int [n_const[HIGH_IF_POSSIBLE]];
// initialize
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
is_high_const[i] = true;
// search
int search_phase = 0;
while(1)
{
n_high_const = n_const[HIGH_PRIORITY];
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(is_high_const[i]) n_high_const++;
}
Jhigh.resize(n_high_const, n_dof);
wJhigh.resize(n_high_const, n_dof);
fb_high.resize(n_high_const);
weight_high.resize(n_high_const);
if(n_const[HIGH_PRIORITY] > 0)
{
// set fb and J of higher priority pins
for(i=0; i<n_const[HIGH_PRIORITY]; i++)
{
fb_high(i) = fb[HIGH_PRIORITY](i);
weight_high(i) = weight[HIGH_PRIORITY](i);
for(j=0; j<n_dof; j++)
{
Jhigh(i, j) = J[HIGH_PRIORITY](i, j);
wJhigh(i, j) = Jhigh(i, j) * weight_high(i) / joint_weights(j);
}
}
}
int count = 0;
// set fb and J of medium priority pins
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(is_high_const[i])
{
fb_high(n_const[HIGH_PRIORITY]+count) = fb[HIGH_IF_POSSIBLE](i);
weight_high(n_const[HIGH_PRIORITY]+count) = weight[HIGH_IF_POSSIBLE](i);
for(j=0; j<n_dof; j++)
{
Jhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j);
wJhigh(n_const[HIGH_PRIORITY]+count, j) = J[HIGH_IF_POSSIBLE](i, j) * weight[HIGH_IF_POSSIBLE](i) / joint_weights(j);
}
count++;
}
}
// singular value decomposition
fMat U(n_high_const, n_high_const), VT(n_dof, n_dof);
fVec s;
int s_size;
if(n_high_const < n_dof) s_size = n_high_const;
else s_size = n_dof;
s.resize(s_size);
wJhigh.svd(U, s, VT);
double condnum_limit = max_condnum * 100.0;
if(s(s_size-1) > s(0)/(max_condnum*condnum_limit))
cond_number = s(0) / s(s_size-1);
else
cond_number = condnum_limit;
if(current_max_condnum < 0.0 || cond_number > current_max_condnum)
{
current_max_condnum = cond_number;
}
if(n_high_const <= n_const[HIGH_PRIORITY]) break;
// remove some constraints
if(cond_number > max_condnum)
{
int reduced = false;
for(i=n_constraints-1; i>=0; i--)
{
if(constraints[i]->enabled &&
constraints[i]->priority == HIGH_IF_POSSIBLE &&
constraints[i]->i_const >= 0 &&
constraints[i]->GetType() == HANDLE_CONSTRAINT &&
is_high_const[constraints[i]->i_const])
{
IKHandle* h = (IKHandle*)constraints[i];
if(search_phase ||
(!search_phase && h->joint->DescendantDOF() > 0))
{
for(j=0; j<constraints[i]->n_const; j++)
{
is_high_const[constraints[i]->i_const + j] = false;
}
constraints[i]->is_dropped = true;
// cerr << "r" << flush;
reduced = true;
break;
}
}
}
if(!reduced) search_phase++;
}
else break;
}
}
else
{
n_high_const = n_const[HIGH_PRIORITY];
Jhigh.resize(n_high_const, n_dof);
wJhigh.resize(n_high_const, n_dof);
fb_high.resize(n_high_const);
weight_high.resize(n_high_const);
if(n_high_const > 0)
{
Jhigh.set(J[HIGH_PRIORITY]);
fb_high.set(fb[HIGH_PRIORITY]);
weight_high.set(weight[HIGH_PRIORITY]);
}
}
#if 0
////
// adjust feedback according to the condition number
if(current_max_condnum > max_condnum)
fb_high.zero();
else
{
double k = (current_max_condnum-max_condnum)/(1.0-max_condnum);
fb_high *= k;
cerr << current_max_condnum << ", " << k << endl;
}
////
////
if(current_max_condnum < 0.0) current_max_condnum = 1.0;
#endif
int n_low_const = n_all_const - n_high_const;
int low_first = 0, count = 0;
fMat Jlow(n_low_const, n_dof);
fVec fb_low(n_low_const);
fVec weight_low(n_low_const);
for(i=0; i<n_const[HIGH_IF_POSSIBLE]; i++)
{
if(!is_high_const[i])
{
fb_low(count) = fb[HIGH_IF_POSSIBLE](i);
weight_low(count) = weight[HIGH_IF_POSSIBLE](i);
for(j=0; j<n_dof; j++)
{
Jlow(count, j) = J[HIGH_IF_POSSIBLE](i, j);
}
count++;
}
}
low_first = count;
double* p = fb_low.data() + low_first;
double* q = fb[LOW_PRIORITY].data();
double* r = weight_low.data() + low_first;
double* s = weight[LOW_PRIORITY].data();
for(i=0; i<n_const[LOW_PRIORITY]; p++, q++, r++, s++, i++)
{
// fb_low(low_first+i) = fb[LOW_PRIORITY](i);
*p = *q;
*r = *s;
double* a = Jlow.data() + low_first + i;
int a_row = Jlow.row();
double* b = J[LOW_PRIORITY].data() + i;
int b_row = J[LOW_PRIORITY].row();
for(j=0; j<n_dof; a+=a_row, b+=b_row, j++)
{
// Jlow(low_first+i, j) = J[LOW_PRIORITY](i, j);
*a = *b;
}
}
if(is_high_const) delete[] is_high_const;
fVec jvel(n_dof); // ³û¼â1¡¦x
fVec jvel0(n_dof); // ¹ó/¡¦¾å
fVec fb_low_0(n_low_const), dfb(n_low_const), y(n_dof);
fMat Jinv(n_dof, n_high_const), W(n_dof, n_dof), JW(n_low_const, n_dof);
fVec w_error(n_low_const), w_norm(n_dof);
// weighted
double damping = 0.1;
// w_error = 1.0;
w_error.set(weight_low);
w_norm = 1.0;
// w_norm.set(joint_weights);
// cerr << "joint_weights = " << joint_weights << endl;
// cerr << "weight_high = " << weight_high << endl;
// cerr << "weight_low = " << weight_low << endl;
#ifdef MEASURE_TIME
clock_t t1 = clock();
#endif
// ¹ó/¡¦¾å¡¦€ÅæéòÉçïàïêvZ
if(n_high_const > 0)
{
for(i=0; i<n_dof; i++)
{
int a_row = wJhigh.row();
double* a = wJhigh.data() + a_row*i;
int b_row = Jhigh.row();
double* b = Jhigh.data() + b_row*i;
double* c = joint_weights.data() + i;
double* d = weight_high.data();
for(j=0; j<n_high_const; a++, b++, d++, j++)
{
// wJhigh(j, i) = Jhigh(j, i) / joint_weights(i);
*a = *b * *d / *c;
}
}
fVec w_fb_high(fb_high);
for(i=0; i<n_high_const; i++)
w_fb_high(i) *= weight_high(i);
Jinv.p_inv(wJhigh);
jvel0.mul(Jinv, w_fb_high);
W.mul(Jinv, wJhigh);
for(i=0; i<n_dof; i++)
{
double* w = W.data() + i;
double a = joint_weights(i);
for(j=0; j<n_dof; w+=n_dof, j++)
{
if(i==j) *w -= 1.0;
*w /= -a;
}
jvel0(i) /= a;
}
JW.mul(Jlow, W);
}
else
{
jvel0.zero();
JW.set(Jlow);
}
#ifdef MEASURE_TIME
clock_t t2 = clock();
high_constraint_time += (double)(t2-t1)/(double)CLOCKS_PER_SEC;
#endif
// Ǥ¡¦#x¥¯¥È¥ë¹à¤ê³×Z
if(n_low_const > 0)
{
fb_low_0.mul(Jlow, jvel0);
dfb.sub(fb_low, fb_low_0);
y.lineq_sr(JW, w_error, w_norm, damping, dfb);
if(n_high_const > 0) jvel.mul(W, y);
else jvel.set(y);
// fVec error(n_low_const);
// error = dfb-Jlow*jvel;
// cerr << dfb*dfb << "->" << error*error << endl;
}
else
{
jvel.zero();
}
// ³û¼â1¡¦x
jvel += jvel0;
#ifdef MEASURE_TIME
clock_t t3 = clock();
low_constraint_time += (double)(t3-t2)/(double)CLOCKS_PER_SEC;
#endif
SetJointVel(jvel);
// cerr << fb_high - Jhigh * jvel << endl;
}
if(current_max_condnum < 0.0) current_max_condnum = 1.0;
return current_max_condnum;
}
