int main(int argc, char** argv)
{
Matrix<int,ColMajor> Ai(rows,cols); // a column-major integer matrix
Matrix<float> Af(rows,cols); // a row-major single precision floating point matrix
Matrix<double> Ad(rows,cols); // a row-major double precision floating point matrix
// grab seed from the current timestamp
unsigned seed = std::chrono::system_clock::now().time_since_epoch().count();
// instance a random number generator
std::default_random_engine generator (seed);
// instance a uniform distribution
std::uniform_real_distribution<double> random(0.0, 1.0);
// note that the loop order is not optimal for the integer matrix
size_t k=1;
for (size_t i=0;i<rows;++i) {
for (size_t j=0;j<cols;++j, k++) {
Ai(i,j) = k;
Af(i,j) = k+random(generator); // add some random stuff for fun
Ad(i,j) = k+random(generator); // add some random stuff for fun
}
}
// print matrices to screen
std::cout << "== Integer matrix ==" << std::endl;
Ai.print();
std::cout << "== Float matrix ==" << std::endl;
Af.print(7);
std::cout << "== Double matrix ==" << std::endl;
Ad.print(16);
return 0;
}
bool GSpringDamperBody::applyForce(bool badd_)
{
if ( pLeftBody == NULL || pRightBody == NULL ) return false;
SE3 T = inv_T_left * Inv(pLeftBody->T_global) * pRightBody->T_global * T_right;
//se3 x(Log(T.GetRotation()), T.GetPosition()); // relative location of the right body from the left body
se3 x(Log(T)); // relative location of the right body from the left body
se3 V_left = Ad(inv_T_left, pLeftBody->V) - Ad(T * inv_T_right, pRightBody->V); // relative velocity of the left body
dse3 F_left; // force to be acting on the left body
for (int i=0; i<6; i++) {
F_left[i] = K[i] * x[i] - C[i] * V_left[i];
}
if ( badd_ ) {
pLeftBody->Fe += dAd(inv_T_left, F_left);
pRightBody->Fe += dAd(T * inv_T_right, -F_left);
} else {
pLeftBody->Fe -= dAd(inv_T_left, F_left);
pRightBody->Fe -= dAd(T * inv_T_right, -F_left);
}
return true;
}
/**
* Compute the LDLT factorization and store pointers to the
* vari's of the matrix entries to be used when chain() is
* called elsewhere.
**/
inline void compute(const Eigen::Matrix<var,R,C> &A) {
Eigen::Matrix<double,R,C> Ad(A.rows(),A.cols());
N_ = A.rows();
_variA.resize(A.rows(),A.cols());
for (size_t j = 0; j < N_; j++) {
for (size_t i = 0; i < N_; i++) {
Ad(i,j) = A(i,j).val();
_variA(i,j) = A(i,j).vi_;
}
}
_ldlt.compute(Ad);
}
void GSystem::update_Jacobian_child_bodies(GBody *pbody_, int idx_, const RMatrix &S_)
{
std::list<GBody *>::iterator iter_pbody;
RMatrix S2;
for (iter_pbody = pbody_->pChildBodies.begin(); iter_pbody != pbody_->pChildBodies.end(); iter_pbody++) {
S2 = Ad((*iter_pbody)->invT, S_);
(*iter_pbody)->Jacobian.Push(0, idx_, S2);
update_Jacobian_child_bodies(*iter_pbody, idx_, S2); // recursive call until reaching end of branch
}
}
TEST(MathMatrix,mdivide_left_ldlt_val) {
stan::math::LDLT_factor<double,-1,-1> ldlt_Ad;
stan::math::matrix_d Ad(2,2);
stan::math::matrix_d I;
Ad << 2.0, 3.0,
3.0, 7.0;
ldlt_Ad.compute(Ad);
ASSERT_TRUE(ldlt_Ad.success());
I = mdivide_left_ldlt(ldlt_Ad,Ad);
EXPECT_NEAR(1.0,I(0,0),1.0E-12);
EXPECT_NEAR(0.0,I(0,1),1.0E-12);
EXPECT_NEAR(0.0,I(1,0),1.0E-12);
EXPECT_NEAR(1.0,I(1,1),1.0e-12);
}
void ConjugateGradient(const int MaxIters,
const double TOL,
const FullMatrix& A,
const dTensor1& rhs,
dTensor1& phi)
{
double DotProd(const dTensor1& avec,
const dTensor1& bvec);
void MatMult(const FullMatrix& A,
const dTensor1& invec,
dTensor1& outvec);
const int size = A.get_NumRows();
int NumIters = 0;
int mflag = 0;
dTensor1 r(size);
dTensor1 d(size);
dTensor1 Ad(size);
for (int i=1; i<=size; i++)
{ phi.set(i, 0.0 ); }
MatMult(A,phi,r);
for (int i=1; i<=size; i++)
{ r.set(i, rhs.get(i)-r.get(i) ); }
for (int i=1; i<=size; i++)
{ d.set(i, r.get(i) ); }
double rhs_norm = sqrt(DotProd(rhs,rhs));
if (fabs(rhs_norm)<=1.0e-12)
{ rhs_norm = 1.0; }
//printf(" rhs_norm = %e\n",rhs_norm);
double rdotr = DotProd(r,r);
//printf(" sqrt(rdotr) = %e\n",sqrt(rdotr));
double rel_res_norm = sqrt(rdotr)/rhs_norm;
if(rel_res_norm < TOL)
{ mflag = 1; }
NumIters = 1;
while(mflag==0)
{
MatMult(A,d,Ad);
double alpha = rdotr/DotProd(d,Ad);
for (int i=1; i<=size; i++)
{ phi.set(i, phi.get(i)+alpha*d.get(i) ); }
for (int i=1; i<=size; i++)
{ r.set(i, r.get(i)-alpha*Ad.get(i) ); }
double rdotr_old = rdotr;
rdotr = DotProd(r,r);
rel_res_norm = sqrt(rdotr)/rhs_norm;
if(rel_res_norm < TOL)
{ mflag = 1; }
//printf(" NumIters = %i, rel_res_norm = %e\n",NumIters,rel_res_norm);
if (NumIters==MaxIters)
{ mflag = 1; }
if (mflag==0)
{
double beta = rdotr/rdotr_old;
for (int i=1; i<=size; i++)
{ d.set(i, r.get(i)+beta*d.get(i) ); }
NumIters = NumIters+1;
}
}
printf(" |----------------------------\n");
printf(" | Conjugate Gradient Results:\n");
printf(" |----------------------------\n");
printf(" | MaxIters = %i\n",MaxIters);
printf(" | TOL = %e\n",TOL);
printf(" | NumIters = %i\n",NumIters);
printf(" | residual = %e\n",rel_res_norm);
printf(" |----------------------------\n");
printf("\n");
}
