/*
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
}
