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);
}
}
}
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);
}
}
}