/* Objective function value calculator: Takes Doc feature matrix and Word feature matrix, as well as ground truth counts, return objective function value and a vector of error value for each case. The objective function is F = acc_error + doc_reg + word_reg = sum_{(doc_id, word_id, cnt):train_vec} (cnt - D(doc_id)*W(word_id))^2 + 0.5*lambda*(|D|^2 + |W|^2) */ double CalcObj(const Mat &D, const Mat &W, const vector<Triplet> &train_vec, const int &begin_idx, const int &end_idx, const double &lambda, const double &mean_cnt, Mat &error, Mat &pred_out){ double acc_error = 0.0, doc_reg = 0.0, word_reg = 0.0; error = Mat(end_idx-begin_idx+1, 1, 0); pred_out = Mat(end_idx-begin_idx+1, 1, 0); for (int i = begin_idx; i <= end_idx; i++){ int doc_id = train_vec[i].doc_id, word_id = train_vec[i].word_id; double cnt = train_vec[i].cnt; // calculate the prediction: inner product of D(doc_id), W(word_id) double predict = 0.0; for (int j=0; j < D.get_m(); j++){ double dd = D.get(doc_id, j), ww = W.get(word_id, j); predict += (dd*ww); doc_reg += sqr(dd); word_reg += sqr(ww); } pred_out[i-begin_idx][0] = predict + mean_cnt; error[i - begin_idx ][0] = pred_out[i-begin_idx][0] - train_vec[i].cnt; // calculate the error between predict and ground truth cnt acc_error += sqr(pred_out[i-begin_idx][0] - train_vec[i].cnt); } // Objective function value double F = acc_error + 0.5*lambda*(doc_reg + word_reg); return F; }
bool equals(const Mat<float>& a, const Mat<float>& b, float precision) { if(a.getLine() == b.getLine() && a.getColumn() == b.getColumn()) { for(int i=a.getLine();i--;) { for(int j=a.getColumn();j--;) { float vala = a.get(i+1,j+1); float valb = b.get(i+1,j+1); if( vala+precision < valb || vala-precision > valb) { return false; } } } } else { std::cerr << "EQUALS : matrices a and b are not of the same sizes." << std::endl; return false; } return true; }
/* Substraction. Throw exception if their dimensions don't match. */ Mat operator -(const Mat& rhs){ if (rhs.get_n()!=n || rhs.get_m()!=m){ throw invalid_argument("Matrix dimensions are not consistent when trying '+' two matrices"); return Mat(); } Mat res = Mat(*this); for (int i=0; i<res.get_n(); i++){ for (int j=0; j<res.get_m(); j++) res[i][j] -= rhs.get(i, j); } return res; }
Mat<float> reshapeV(const Mat<float>& m) { int line = m.getLine(); int column = m.getColumn(); Mat<float> r(line*column,1); for(int i=1;i<=line;i++) { for(int j=1;j<=column;j++) { r.set( m.get(i,j), (i-1)*column+j, 1); } } return r; }
Mat<float> closestPointLOfBOXGivenPointL(RigidBody& rb, const Mat<float>& pointL) { BoxShape& box = (BoxShape&)rb.getShapeReference(); Mat<float> min((float)0,3,1); Mat<float> max((float)0,3,1); min.set( -box.getHeight()/2.0f, 1,1); min.set( -box.getWidth()/2.0f, 2,1); min.set( -box.getDepth()/2.0f, 3,1); max.set( -min.get(1,1), 1,1); max.set( -min.get(2,1), 2,1); max.set( -min.get(3,1), 3,1); Mat<float> clpL(3,1); //compute the closest point by projecting the point to the closest face using Voronoi regions approach. bool change = false; for(int i=1;i<=3;i++) { float v = pointL.get(i,1); if( v < min.get(i,1)) { v = min.get(i,1); change = true; } if( v > max.get(i,1)) { v = max.get(i,1); change = true; } clpL.set( v, i,1); } if(!change) { //then it means that the given point is to be inside the BoxShape : #ifdef debug std::cout << "COLLISION DETECTOR : CLPLL : inside point : " << std::endl; operatorL(operatorL(clpL,pointL), rb.getPointInWorld(pointL)).afficher(); #endif } return clpL; }
QString FightSheet::generateData() { Style *s = new Style( object->get("style").toInt(), keeper ); setReportFile(s->get("name").toLower() + ".html"); setReportPath("/../share/reports/fightsheet"); Wrestler *red = new Wrestler(object->get("red").toInt(), keeper); Geo *geo_r = new Geo(red->get("geo").toInt(), keeper); Wrestler *blue = new Wrestler(object->get("blue").toInt(), keeper); Geo *geo_b = new Geo(blue->get("geo").toInt(), keeper); Weight *wh = new Weight( object->get("weight").toInt(), keeper ); Competition *c = new Competition(object->get("competition").toInt(), keeper); Mat *mat = new Mat(object->get("mat").toInt(), keeper); CRound *cround = new CRound(object->get("cround").toInt(), keeper); CTour *ctour = new CTour(object->get("ctour").toInt(), keeper); QDate start = QDate::fromString(c->get("start"), QString("yyyy-MM-dd")); QDate stop = QDate::fromString(c->get("stop"), QString("yyyy-MM-dd")); QString date; if ( start.month() == stop.month() ) { date = QString("%1-%2/%3/%4").arg(start.day()).arg(stop.day()).arg(start.month()).arg(start.year()); } else { date = start.toString("dd/MM/yyyy") + " - " + stop.toString("dd/MM/yyyy"); } vars.insert("{red.title}", red->get("title")); vars.insert("{red.geo}", geo_r->get("title")); vars.insert("{blue.title}", blue->get("title")); vars.insert("{blue.geo}", geo_b->get("title")); vars.insert("{style}", s->get("name")); vars.insert("{weight}", wh->get("title")); vars.insert("{competition}", c->get("title")); vars.insert("{mat}", mat->get("title")); vars.insert("{round}", cround->get("title")); vars.insert("{place}", ctour->get("title")); vars.insert("{num}", object->get("num")); vars.insert("{date}", date); TQueryMap opt; opt.insert("competition", keeper->prepareParam(Equal, c->get("id"))); QList<QVariant> id_list; if ( red->get("id").toInt() > 0 ) id_list << red->get("id"); if ( blue->get("id").toInt() > 0 ) id_list << blue->get("id"); opt.insert("wrestler", keeper->prepareParam(And | InSet, id_list )); opt.insert("style", keeper->prepareParam(And | Equal, s->get("id"))); opt.insert("weight", keeper->prepareParam(And | Equal, wh->get("id"))); QList<QVariant> list = keeper->getListOfFields(OCompetitionWrestler, QStringList() << "wrestler" << "sorder", opt); for( int i = 0; i < list.size(); i++) { QStringList row = list.at(i).toStringList(); if ( red->get("id").toInt() == row.at(0).toInt() ) vars.insert("{red.num}", row.at(1)); if ( blue->get("id").toInt() == row.at(0).toInt() ) vars.insert("{blue.num}", row.at(1)); } delete red; delete geo_r; delete geo_b; delete blue; delete s; delete wh; delete mat; delete cround; delete ctour; delete c; return applyTemplateVars(loadTemplate(reportFile), vars); }
int main() { { // Test lower_bound and min_max using jcui::algorithm::lower_bound; using jcui::algorithm::min_max; { int xa[] = {}; int ya[] = {}; vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya)); int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin(); eq(0, index, 0); eq(0, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 0); } { int xa[] = {3, 4, 5, 6}; int ya[] = {5, 4, 3, 1}; vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya)); int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin(); eq(1, index, 1); eq(1, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 4); } { int xa[] = {3, 3, 5, 6}; int ya[] = {5, 4, 3, 1}; vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya)); int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin(); eq(2, index, 2); eq(2, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 4); } { int xa[] = {13, 14, 15, 16}; int ya[] = {5, 4, 3, 1}; vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya)); int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin(); eq(3, index, 0); eq(3, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 13); } { int xa[] = {3, 4, 5, 6}; int ya[] = {15, 14, 13, 11}; vector<int> x(xa, xa + ARRAYSIZE(xa)), y(ya, ya + ARRAYSIZE(ya)); int index = lower_bound(x.begin(), x.end(), y.begin(), y.end(), less<int>()) - x.begin(); eq(4, index, 4); eq(4, min_max(x.begin(), x.end(), y.begin(), y.end(), 0), 11); } } { // For SparseMat using jcui::algorithm::SparseMat; { SparseMat<int> a(100, 100), b(100, 100); SparseMat<int> c = a * b; eq(c.get(3, 10), 0); eq(c.get(0, 0), 0); } { // a = [1, 2; 3, 0], b = [6, 0, 0; 0, 1, 4], a * b = [6, 2, 8; 18, 0, 0] SparseMat<int> a(100, 100), b(100, 100); a.set(0, 0, 1); a.set(0, 1, 2); a.set(1, 0, 3); b.set(0, 0, 6); b.set(1, 1, 1); b.set(1, 2, 4); SparseMat<int> c = a * b; eq(c.get(0, 0), 6); eq(c.get(0, 1), 2); eq(c.get(0, 2), 8); eq(c.get(1, 0), 18); eq(c.get(1, 1), 0); eq(c.get(1, 2), 0); } { // a = [1, 2; 3, 0] SparseMat<long> a(100, 100); a.set(0, 0, 1); a.set(0, 1, 2); a.set(1, 0, 3); SparseMat<long> c = pow(a, 17); eq(c.get(0, 0), 77431669L); eq(c.get(0, 1), 51708494L); eq(c.get(1, 0), 77562741L); eq(c.get(1, 1), 51577422L); } { // a = [1, 2; 3, 0] int N = 10; SparseMat<float> a(N, N); for (int i = 0; i < N; ++i) { a.set(i, i, 0.9f); a.set(i, (i + 1) % N, 0.05f); a.set(i, (i + N - 1) % N, 0.05f); } SparseMat<float> c = pow(a, 20000); for (int i = 0; i < N; ++i) { for (int j = 0; j < N; ++j) { eq(fabs(c.get(i, j) - 0.1f) < 1e-3f, true); } } } { // a = [1, 2; 3, 0] SparseMat<long> a(2, 2); a.set(0, 0, 1); a.set(0, 1, 2); a.set(1, 0, 3); long xa[] = {4, 5}; vector<long> x(xa, xa + ARRAYSIZE(xa)); vector<long> y = a * x; eq(y[0], 14L); eq(y[1], 12L); } { // a = [1, 2, 1; 3, 3, 0] SparseMat<int> a(2, 3); a.set(0, 0, 1); a.set(0, 1, 2); a.set(0, 2, 1); a.set(1, 0, 3); a.set(1, 1, 3); vector<int> y = SparseMat<int>::row_sum(a); eq(y[0], 4); eq(y[1], 5); eq(y[2], 1); } { // a = [1, 2, 1; 3, 3, 0] SparseMat<float> a(2, 3); a.set(0, 0, 1); a.set(0, 1, 2); a.set(0, 2, 1); a.set(1, 0, 3); a.set(1, 1, 3); a.normalize_by_row_sum(); eq(a.get(0, 0), 0.25f); eq(a.get(0, 1), 0.4f); eq(a.get(0, 2), 1.0f); eq(a.get(1, 0), 0.75f); eq(a.get(1, 1), 0.6f); eq(a.get(1, 2), 0.0f); } } { using jcui::algorithm::RingBuffer; { RingBuffer<int> R(3); R.push_back(3); eq(R.get(0), 3); eq(R.get(-10), 0); eq(R.get(10), 0); eq(R.get(3), 0); R.push_back(2); eq(R.get(-1), 3); eq(R.get(0), 2); R.push_back(1); eq(R.get(-2), 3); eq(R.get(-1), 2); eq(R.get(0), 1); R.push_back(7); eq(R.get(-2), 2); eq(R.get(-1), 1); eq(R.get(0), 7); eq(R.get(-3), 0); eq(R.get(3), 0); } } { using jcui::algorithm::Mat; { // a = [1, 2; 3, 0], b = [6, 0, 0; 0, 1, 4], a * b = [6, 2, 8; 18, 0, 0] Mat<int> a(100, 100), b(100, 100); a.set(0, 0, 1); a.set(0, 1, 2); a.set(1, 0, 3); b.set(0, 0, 6); b.set(1, 1, 1); b.set(1, 2, 4); Mat<int> c = a * b; eq(c.get(0, 0), 6); eq(c.get(0, 1), 2); eq(c.get(0, 2), 8); eq(c.get(1, 0), 18); eq(c.get(1, 1), 0); eq(c.get(1, 2), 0); } { // a = [1, 2; 3, 0] Mat<long> a(100, 100); a.set(0, 0, 1); a.set(0, 1, 2); a.set(1, 0, 3); Mat<long> c = pow(a, 17); eq(c.get(0, 0), 77431669L); eq(c.get(0, 1), 51708494L); eq(c.get(1, 0), 77562741L); eq(c.get(1, 1), 51577422L); eq(c.get(10, 20), 0L); } { Mat<long> a(2, 2); a.set(0, 0, 1); a.set(0, 1, 2); a.set(1, 0, 3); long xa[] = {4, 5}; vector<long> x(xa, xa + ARRAYSIZE(xa)); vector<long> y = a * x; eq(y[0], 14L); eq(y[1], 12L); } { // a = [1, 2, 1; 3, 3, 0] Mat<float> a(2, 3); a.set(0, 0, 1); a.set(0, 1, 2); a.set(0, 2, 1); a.set(1, 0, 3); a.set(1, 1, 3); a.normalize_by_row_sum(); eq(a.get(0, 0), 0.25f); eq(a.get(0, 1), 0.4f); eq(a.get(0, 2), 1.0f); eq(a.get(1, 0), 0.75f); eq(a.get(1, 1), 0.6f); eq(a.get(1, 2), 0.0f); } } { using jcui::algorithm::SparseMat; using jcui::algorithm::SparseMatCSR; { // a = [1, 2, 1; 3, 3, 0] SparseMat<int> a(2, 3); a.set(0, 0, 1); a.set(0, 1, 2); a.set(0, 2, 1); a.set(1, 0, 3); a.set(1, 1, 3); SparseMatCSR<int> b(a); int v[] = {1, 2, 1, 3, 3}; eq(b.v, VI(v)); int col[] = {0, 1, 2, 0, 1}; eq(b.col, VI(col)); int cum_n[] = {0, 3, 5}; eq(b.cum_n, VI(cum_n)); int c[] = {0, 5, 7}; int res[] = {17, 15}; eq(b * VI(c), VI(res)); } } { using namespace jcui::algorithm; { vector<vector<int> > res = split_no_repeat(8, 7); eq((int)res.size(), 5); } { vector<vector<int> > res = split_no_repeat(10, 9); eq((int)res.size(), 9); } } }
/* Copy constructor. Deep copy. */ Mat(const Mat &cp){ InitMat(cp.get_n(),cp.get_m()); for (int i=0; i<n; i++) for (int j=0; j<m; j++) arr[i][j] = cp.get(i,j); }
void SpeechRec::SentenceBasedNormalization(Mat<float> *mat) { // mat->saveAscii("c:\\before"); // sentence mean and variance normalization bool mean_norm = C.GetBool("offlinenorm", "sent_mean_norm"); bool var_norm = C.GetBool("offlinenorm", "sent_var_norm"); if(mean_norm || var_norm) { Mat<float> mean; // mean calcualtion mat->sumColumns(mean); mean.div((float)mat->rows()); // mean norm int i, j; for(i = 0; i < mat->columns(); i++) mat->sub(0, mat->rows() - 1, i, i, mean.get(0, i)); if(var_norm) { // variance calculation Mat<float> var; var.init(mean.rows(), mean.columns()); var.set(0.0f); for(i = 0; i < mat->columns(); i++) { for(j = 0; j < mat->rows(); j++) { float v = mat->get(j, i); var.add(0, i, v * v); } } var.div((float)mat->rows()); var.sqrt(); // lower threshold float lowerThr = C.GetFloat("melbanks", "sent_std_thr"); var.lowerLimit(lowerThr); // variance norm for(i = 0; i < mat->columns(); i++) mat->mul(0, mat->rows() - 1, i, i, 1.0f / var.get(0, i)); // add mean if not mean norm if(!mean_norm) { for(i = 0; i < mat->columns(); i++) mat->add(0, mat->rows() - 1, i, i, mean.get(0, i)); } } } // sentence maximum normalization bool max_norm = C.GetBool("offlinenorm", "sent_max_norm"); bool channel_max_norm = C.GetBool("offlinenorm", "sent_chmax_norm"); if(max_norm || channel_max_norm) { Mat<float> max; max.init(1, mat->columns()); max.set(-9999.9f); int i, j; for(i = 0; i < mat->columns(); i++) { for(j = 0; j < mat->rows(); j++) { float v = mat->get(j, i); if(v > max.get(0, i)) { max.set(0, i, v); } } } // global sentence maximum normalization if(max_norm) { float global_max = -9999.9f; for(i = 0; i < max.columns(); i++) { if(max.get(0, i) > global_max) { global_max = max.get(0, i); } max.set(global_max); } } for(i = 0; i < mat->columns(); i++) { for(j = 0; j < mat->rows(); j++) { mat->set(j, i, mat->get(j, i) - max.get(0, i)); } } } }
void SimultaneousImpulseBasedConstraintSolverStrategy::Solve(float dt, std::vector<std::unique_ptr<IConstraint> >& c, Mat<float>& q, Mat<float>& qdot, SparseMat<float>& invM, SparseMat<float>& S, const Mat<float>& Fext ) { //std::cout << "STATE :" << std::endl; //q.afficher(); Mat<float> qdotminus(qdot); this->dt = dt; //computeConstraintsJacobian(c); Mat<float> tempInvMFext( dt*(invM * Fext) ) ; //qdot += tempInvMFext; //computeConstraintsJacobian(c,q,qdot); computeConstraintsANDJacobian(c,q,qdot); //BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function.... //std::cout << "Constraints : norme = " << norme2(C) << std::endl; //C.afficher(); Mat<float> tConstraintsJacobian( transpose(constraintsJacobian) ); //std::cout << "t Constraints Jacobian :" << std::endl; //tConstraintsJacobian.afficher(); //PREVIOUS METHOD : //-------------------------------- //David Baraff 96 STAR.pdf Interactive Simulation of Rigid Body Dynamics in Computer Graphics : Lagrange Multipliers Method : //Construct A : /* Mat<float> A( (-1.0f)*tConstraintsJacobian ); Mat<float> M( invGJ( invM.SM2mat() ) ); A = operatorL( M, A); A = operatorC( A , operatorL( constraintsJacobian, Mat<float>((float)0,constraintsJacobian.getLine(), constraintsJacobian.getLine()) ) ); */ //---------------------------- Mat<float> A( constraintsJacobian * invM.SM2mat() * tConstraintsJacobian ); //--------------------------- Mat<float> invA( invGJ(A) );//invM*tConstraintsJacobian ) * constraintsJacobian ); //Construct b and compute the solution. //---------------------------------- //Mat<float> tempLambda( invA * operatorC( Mat<float>((float)0,invA.getLine()-constraintsJacobian.getLine(),1) , (-1.0f)*(constraintsJacobian*(invM*Fext) + offset) ) ); //----------------------------------- Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobian*tempInvMFext + offset) ) ); //----------------------------------- //Solutions : //------------------------------------ //lambda = extract( &tempLambda, qdot.getLine()+1, 1, tempLambda.getLine(), 1); //if(isnanM(lambda)) // lambda = Mat<float>(0.0f,lambda.getLine(),lambda.getColumn()); //Mat<float> udot( extract( &tempLambda, 1,1, qdot.getLine(), 1) ); //------------------------------------ lambda = tempLambda; Mat<float> udot( tConstraintsJacobian * tempLambda); //------------------------------------ if(isnanM(udot)) udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn()); float clampingVal = 1e4f; for(int i=1;i<=udot.getLine();i++) { if(udot.get(i,1) > clampingVal) { udot.set( clampingVal,i,1); } } #ifdef debuglvl1 std::cout << " SOLUTIONS : udot and lambda/Pc : " << std::endl; transpose(udot).afficher(); transpose(lambda).afficher(); transpose( tConstraintsJacobian*lambda).afficher(); #endif //Assumed model : //qdot = tempInvMFext + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1); //qdot = tempInvMFext + udot; qdot += tempInvMFext + invM*udot; //qdot += invM*udot; //qdot += tempInvMFext + udot; float clampingValQ = 1e3f; for(int i=1;i<=qdot.getLine();i++) { if( fabs_(qdot.get(i,1)) > clampingValQ) { qdot.set( clampingValQ * fabs_(qdot.get(i,1))/qdot.get(i,1),i,1); } } //qdot = udot; //Assumed model if the update of the integration is applied after that constraints solver : //qdot += dt*extract( &tempLambda, 1,1, qdot.getLine(), 1);//+tempInvMFext Mat<float> t( dt*( S*qdot ) ); float clampQuat = 1e-1f; float idxQuat = 3; while(idxQuat < t.getLine()) { for(int i=1;i<4;i++) { if( fabs_(t.get(idxQuat+i,1)) > clampQuat) { t.set( clampQuat*(t.get(idxQuat+i,1))/t.get(idxQuat+i,1), idxQuat+i,1); } } idxQuat += 7; } //the update is done by the update via an accurate integration and we must construct q and qdot at every step //q += t; //-------------------------------------- //let us normalize each quaternion : /* idxQuat = 3; while(idxQuat < q.getLine()) { float scaler = q.get( idxQuat+4,1); if(scaler != 0.0f) { for(int i=1;i<=4;i++) { q.set( q.get(idxQuat+i,1)/scaler, idxQuat+i,1); } } idxQuat += 7; } */ //-------------------------------------- #ifdef debuglvl2 //std::cout << " computed Pc : " << std::endl; //(tConstraintsJacobian*tempLambda).afficher(); //std::cout << " q+ : " << std::endl; //transpose(q).afficher(); std::cout << " qdot+ : " << std::endl; transpose(qdot).afficher(); std::cout << " qdotminus : " << std::endl; transpose(qdotminus).afficher(); #endif #ifdef debuglvl3 std::cout << "SOME VERIFICATION ON : J*qdot + c = 0 : " << std::endl; transpose(constraintsJacobian*qdot+offset).afficher(); float normC = (transpose(C)*C).get(1,1); Mat<float> Cdot( constraintsJacobian*qdot); float normCdot = (transpose(Cdot)*Cdot).get(1,1); float normQdot = (transpose(qdot)*qdot).get(1,1); //rs->ltadd(std::string("normC"),normC); //rs->ltadd(std::string("normCdot"),normCdot); rs->ltadd(std::string("normQdot"),normQdot); char name[5]; for(int i=1;i<=t.getLine();i++) { sprintf(name,"dq%d",i); rs->ltadd(std::string(name), t.get(i,1)); } rs->tWriteFileTABLE(); #endif //END OF PREVIOUS METHOD : //-------------------------------- //-------------------------------- //-------------------------------- //Second Method : /* //According to Tonge Richar's Physucs For Game pdf : Mat<float> tempLambda( (-1.0f)*invGJ( constraintsJacobian*invM.SM2mat()*tConstraintsJacobian)*constraintsJacobian*qdot ); qdot += invM*tConstraintsJacobian*tempLambda; //qdot += tempInvMFext; //contraints not satisfied. //qdot += tempInvMFext; //qdot+ = qdot- + dt*M-1Fext; //Mat<float> qdotreal( qdot + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1) ); //qdotreal = qdot+ + Pc; Mat<float> t( dt*( S*qdot ) ); q += t; */ //-------------------------------- //-------------------------------- //End of second method... //-------------------------------- //-------------------------------- //-------------------------------- //THIRD METHOD : //-------------------------------- //With reference to A Unified Framework for Rigid Body Dynamics Chap. 4.6.2.Simultaneous Force-based methods : //which refers to Bara96 : /* Mat<float> iM(invM.SM2mat()); Mat<float> b((-1.0f)*constraintsJacobian*iM*Fext+offset); Mat<float> tempLambda( invGJ( constraintsJacobian*iM*tConstraintsJacobian) * b ); //Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda+Fext)); Mat<float> qdoubledot(iM*(tConstraintsJacobian*tempLambda)); qdot += dt*qdoubledot; //qdot += tempInvMFext; //contraints not satisfied. //qdot += tempInvMFext; //qdot+ = qdot- + dt*M-1Fext; //Mat<float> qdotreal( qdot + dt*extract( &tempLambda, 1,1, qdot.getLine(), 1) ); //qdotreal = qdot+ + Pc; Mat<float> t( dt*( S*qdot ) ); q += t; std::cout << " computed Pc : " << std::endl; (tConstraintsJacobian*tempLambda).afficher(); std::cout << " q+ : " << std::endl; q.afficher(); std::cout << " qdot+ : " << std::endl; qdot.afficher(); */ //END OF THIRD METHOD : //-------------------------------- //S.print(); //std::cout << " computed Pc : " << std::endl; //(tConstraintsJacobian*lambda).afficher(); //std::cout << " delta state = S * qdotreal : " << std::endl; //t.afficher(); //std::cout << " S & qdotreal : " << std::endl; //S.print(); //qdot.afficher(); //std::cout << "invM*Fext : " << std::endl; //tempInvMFext.afficher(); //temp.afficher(); //(constraintsJacobian*(invM*Fext)).afficher(); //(invM*Fext).afficher(); //std::cout << " A : " << std::endl; //A.afficher(); //std::cout << " SVD A*tA : S : " << std::endl; //SVD<float> instanceSVD(A*transpose(A)); //instanceSVD.getS().afficher(); //std::cout << " invA : " << std::endl; //invA.afficher(); //std::cout << " LAMBDA : " << std::endl; //lambda.afficher(); //std::cout << " qdot+ & qdot- : " << std::endl; //qdot.afficher(); //qdotminus.afficher(); //std::cout << " q+ : " << std::endl; //q.afficher(); #ifdef debuglvl4 //BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function.... std::cout << "tConstraints : norme = " << norme2(C) << std::endl; transpose(C).afficher(); std::cout << "Cdot : " << std::endl; (constraintsJacobian*qdot).afficher(); std::cout << " JACOBIAN : " << std::endl; //transpose(constraintsJacobian).afficher(); constraintsJacobian.afficher(); std::cout << " Qdot+ : " << std::endl; transpose(qdot).afficher(); #endif //BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function.... //std::cout << "Constraints : norme = " << norme2(C) << std::endl; //C.afficher(); }
void IterativeImpulseBasedConstraintSolverStrategy::computeConstraintsANDJacobian(std::vector<std::unique_ptr<IConstraint> >& c, const Mat<float>& q, const Mat<float>& qdot, const SparseMat<float>& invM) { //------------------------------------- //------------------------------------- //------------------------------------- size_t size = c.size(); int n = sim->simulatedObjects.size(); float baumgarteBAS = 0.0f;//1e-1f; float baumgarteC = -2e0f; float baumgarteH = 0.0f;//1e-1f; //--------------- // RESETTING : constraintsC.clear(); constraintsJacobians.clear(); constraintsOffsets.clear(); constraintsIndexes.clear(); constraintsInvM.clear(); constraintsV.clear(); //---------------------- if( size > 0) { for(int k=0;k<size;k++) { int idA = ( c[k]->rbA.getID() ); int idB = ( c[k]->rbB.getID() ); std::vector<int> indexes(2); //indexes are set during the creation of the simulation and they begin at 0. indexes[0] = idA; indexes[1] = idB; constraintsIndexes.push_back( indexes ); //--------------------------- //Constraint : c[k]->computeJacobians(); Mat<float> tJA(c[k]->getJacobianA()); Mat<float> tJB(c[k]->getJacobianB()); Mat<float> tC(c[k]->getConstraint()); constraintsC.push_back( tC ); int nbrlineJ = tJA.getLine(); Mat<float> JacobianAB( operatorL(tJA, tJB) ); constraintsJacobians.push_back( JacobianAB ); //---------------------------------------- //BAUMGARTE STABILIZATION //---------------------------------------- //Contact offset : if( c[k]->getType() == CTContactConstraint) { //---------------------------------------- //SLOP METHOD : /* float slop = 1e0f; float pdepth = ((ContactConstraint*)(c[k].get()))->penetrationDepth; std::cout << " ITERATIVE SOLVER :: CONTACT : pDepth = " << pdepth << std::endl; tC *= baumgarteC/this->dt * fabs_(fabs_(pdepth)-slop); */ //---------------------------------------- //---------------------------------------- //DEFAULT METHOD : tC *= baumgarteC/this->dt; //---------------------------------------- //---------------------------------------- //METHOD 2 : /* float restitFactor = ( (ContactConstraint*) (c[k].get()) )->getRestitutionFactor(); std::cout << " ITERATIVE SOLVER :: CONTACT : restitFactor = " << restitFactor << std::endl; Mat<float> Vrel( ( (ContactConstraint*) (c[k].get()) )->getRelativeVelocity() ); Mat<float> normal( ( (ContactConstraint*) (c[k].get()) )->getNormalVector() ); std::cout << " ITERATIVE SOLVER :: CONTACT : Normal vector : " << std::endl; transpose(normal).afficher(); tC += restitFactor * transpose(Vrel)*normal; */ //---------------------------------------- std::cout << " ITERATIVE SOLVER :: CONTACT : Contact Constraint : " << std::endl; transpose(tC).afficher(); std::cout << " ITERATIVE SOLVER :: CONTACT : Relative Velocity vector : " << std::endl; transpose(( (ContactConstraint*) (c[k].get()) )->getRelativeVelocity()).afficher(); //std::cout << " ITERATIVE SOLVER :: CONTACT : First derivative of Contact Constraint : " << std::endl; //(transpose(tJA)*).afficher(); } //BAS JOINT : if( c[k]->getType() == CTBallAndSocketJoint) { tC *= baumgarteBAS/this->dt; } //HINGE JOINT : if( c[k]->getType() == CTHingeJoint) { tC *= baumgarteH/this->dt; } //BAUMGARTE OFFSET for the moments... constraintsOffsets.push_back( tC ); //---------------------------------------- //---------------------------------------- //------------------- // invM matrixes : Mat<float> invmij(0.0f,12,12); for(int k=0;k<=1;k++) { for(int i=1;i<=6;i++) { for(int j=1;j<=6;j++) { invmij.set( invM.get( indexes[k]*6+i, indexes[k]*6+j), k*6+i,k*6+j); } } } constraintsInvM.push_back( invmij); //------------------- // Vdot matrixes : Mat<float> vij(0.0f,12,1); for(int k=0;k<=1;k++) { for(int i=1;i<=6;i++) { vij.set( qdot.get( indexes[k]*6+i, 1), k*6+i,1); } } constraintsV.push_back( vij); } } }
/* FORCE BASED : */ void SimultaneousImpulseBasedConstraintSolverStrategy::SolveForceBased(float dt, std::vector<std::unique_ptr<IConstraint> >& c, Mat<float>& q, Mat<float>& qdot, SparseMat<float>& invM, SparseMat<float>& S, const Mat<float>& Fext ) { Mat<float> qdotminus(qdot); this->dt = dt; Mat<float> tempInvMFext( dt*(invM * Fext) ) ; computeConstraintsANDJacobian(c,q,qdot); Mat<float> tConstraintsJacobian( transpose(constraintsJacobian) ); Mat<float> A( constraintsJacobian * invM.SM2mat() * tConstraintsJacobian ); //--------------------------- Mat<float> invA( invGJ(A) ); //Construct b and compute the solution. //----------------------------------- Mat<float> tempLambda( invA * ((-1.0f)*(constraintsJacobian*tempInvMFext + offset) ) ); //----------------------------------- //Solutions : //------------------------------------ lambda = tempLambda; Mat<float> udot( tConstraintsJacobian * tempLambda); //------------------------------------ if(isnanM(udot)) udot = Mat<float>(0.0f,udot.getLine(),udot.getColumn()); float clampingVal = 1e4f; for(int i=1;i<=udot.getLine();i++) { if(udot.get(i,1) > clampingVal) { udot.set( clampingVal,i,1); } } #ifdef debuglvl1 std::cout << " SOLUTIONS : udot and lambda/Pc : " << std::endl; transpose(udot).afficher(); transpose(lambda).afficher(); transpose( tConstraintsJacobian*lambda).afficher(); #endif //Assumed model : qdot += tempInvMFext + dt*(invM*udot); float clampingValQ = 1e3f; for(int i=1;i<=qdot.getLine();i++) { if( fabs_(qdot.get(i,1)) > clampingValQ) { qdot.set( clampingValQ * fabs_(qdot.get(i,1))/qdot.get(i,1),i,1); } } //-------------------------------------- #ifdef debuglvl2 //std::cout << " computed Pc : " << std::endl; //(tConstraintsJacobian*tempLambda).afficher(); //std::cout << " q+ : " << std::endl; //transpose(q).afficher(); std::cout << " qdot+ : " << std::endl; transpose(qdot).afficher(); std::cout << " qdotminus : " << std::endl; transpose(qdotminus).afficher(); #endif //END OF PREVIOUS METHOD : //-------------------------------- #ifdef debuglvl4 //BAUMGARTE STABILIZATION has been handled in the computeConstraintsANDJacobian function.... std::cout << "tConstraints : norme = " << norme2(C) << std::endl; transpose(C).afficher(); std::cout << "Cdot : " << std::endl; (constraintsJacobian*qdot).afficher(); std::cout << " JACOBIAN : " << std::endl; //transpose(constraintsJacobian).afficher(); constraintsJacobian.afficher(); std::cout << " Qdot+ : " << std::endl; transpose(qdot).afficher(); #endif }