C++ Matrix3d::norm方法代码示例

bool so3bracket_tests()
{
  bool failed = false;
  vector<Vector3d> vecs;
  vecs.push_back(Vector3d(0,0,0));
  vecs.push_back(Vector3d(1,0,0));
  vecs.push_back(Vector3d(0,1,0));
  vecs.push_back(Vector3d(M_PI_2,M_PI_2,0.0));
  vecs.push_back(Vector3d(-1,1,0));
  vecs.push_back(Vector3d(20,-1,0));
  vecs.push_back(Vector3d(30,5,-1));
  for (uint i=0; i<vecs.size(); ++i)
  {
    for (uint j=0; j<vecs.size(); ++j)
    {
      Vector3d res1 = SO3::lieBracket(vecs[i],vecs[j]);
      Matrix3d mat =
          SO3::hat(vecs[i])*SO3::hat(vecs[j])
          -SO3::hat(vecs[j])*SO3::hat(vecs[i]);
      Vector3d res2 = SO3::vee(mat);
      Vector3d resDiff = res1-res2;
      if (resDiff.norm()>SMALL_EPS)
      {
        cerr << "SO3 Lie Bracket Test" << endl;
        cerr  << "Test case: " << i << ", " <<j<< endl;
        cerr << res1-res2 << endl;
        cerr << endl;
        failed = true;
      }
    }

    Vector3d omega = vecs[i];
    Matrix3d exp_x = SO3::exp(omega).matrix();
    Matrix3d expmap_hat_x = (SO3::hat(omega)).exp();
    Matrix3d DiffR = exp_x-expmap_hat_x;
    double nrm = DiffR.norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "expmap(hat(x)) - exp(x)" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << exp_x <<endl;
      cerr << expmap_hat_x <<endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }
  }
  return failed;


}

double JohnsonCookFailureStrain(const double p, const Matrix3d Sdev, const double d1, const double d2, const double d3,
                const double d4, const double epdot0, const double epdot) {



        double vm = sqrt(3. / 2.) * Sdev.norm(); // von-Mises equivalent stress
        if (vm < 0.0) {
                cout << "this is sdev " << endl << Sdev << endl;
                printf("vm=%f < 0.0, surely must be an error\n", vm);
                exit(1);
        }

        // determine stress triaxiality
        double triax = p / (vm + 0.01 * fabs(p)); // have softening in denominator to avoid divison by zero
        if (triax < 0.0) {
                triax = 0.0;
        } else if (triax > 3.0) {
                triax = 3.0;
        }

        // Johnson-Cook failure strain, dependence on stress triaxiality
        double jc_failure_strain = d1 + d2 * exp(d3 * triax);

        // include strain rate dependency if parameter d4 is defined and current plastic strain rate exceeds reference strain rate
        if (d4 > 0.0) { //
                if (epdot > epdot0) {
                        double epdot_ratio = epdot / epdot0;
                        jc_failure_strain *= (1.0 + d4 * log(epdot_ratio));
                        //printf("epsdot=%f, epsdot0=%f, factor = %f\n", epdot, epdot0, (1.0 + d4 * log(epdot_ratio)));
                        //exit(1);

                }
        }

        return jc_failure_strain;

}

bool scso3bracket_tests()
{
  bool failed = false;
  vector<Vector4d> vecs;
  Vector4d tmp;
  tmp << 0,0,0,0;
  vecs.push_back(tmp);
  tmp << 1,0,0,0;
  vecs.push_back(tmp);
  tmp << 1,0,0,0.1;
  vecs.push_back(tmp);
  tmp << 0,1,0,0.1;
  vecs.push_back(tmp);
  tmp << 0,0,1,-0.1;
  vecs.push_back(tmp);
  tmp << -1,1,0,-0.1;
  vecs.push_back(tmp);
  tmp << 20,-1,0,2;
  vecs.push_back(tmp);
  for (unsigned int i=0; i<vecs.size(); ++i)
  {
    Vector4d resDiff = vecs[i] - ScSO3::vee(ScSO3::hat(vecs[i]));
    if (resDiff.norm()>SMALL_EPS)
    {
      cerr << "Hat-vee Test" << endl;
      cerr  << "Test case: " << i <<  endl;
      cerr << resDiff.transpose() << endl;
      cerr << endl;
    }

    for (unsigned int j=0; j<vecs.size(); ++j)
    {
      Vector4d res1 = ScSO3::lieBracket(vecs[i],vecs[j]);
      Matrix3d hati = ScSO3::hat(vecs[i]);
      Matrix3d hatj = ScSO3::hat(vecs[j]);

      Vector4d res2 = ScSO3::vee(hati*hatj-hatj*hati);
      Vector4d resDiff = res1-res2;
      if (resDiff.norm()>SMALL_EPS)
      {
        cerr << "ScSO3 Lie Bracket Test" << endl;
        cerr  << "Test case: " << i << ", " <<j<< endl;
        cerr << vecs[i].transpose() << endl;
        cerr << vecs[j].transpose() << endl;
        cerr << resDiff.transpose() << endl;
        cerr << endl;
        failed = true;
      }
    }



    Vector4d omega = vecs[i];
    Matrix3d exp_x = ScSO3::exp(omega).matrix();
    Matrix3d expmap_hat_x = (ScSO3::hat(omega)).exp();
    Matrix3d DiffR = exp_x-expmap_hat_x;
    double nrm = DiffR.norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "expmap(hat(x)) - exp(x)" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << exp_x <<endl;
      cerr << expmap_hat_x <<endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }
  }
  return failed;
}

bool scso3explog_tests()
{
  double pi = 3.14159265;
  vector<ScSO3> omegas;
  omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0, 1.)));
  omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, -1.0, 1.1)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0., 1.1)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0.00001)));
  omegas.push_back(ScSO3::exp(Vector4d(0., 0., 0.00001, 0)));
  omegas.push_back(ScSO3::exp(Vector4d(pi, 0, 0, 0.9)));
  omegas.push_back(ScSO3::exp(Vector4d(0.2, 0.5, 0.0,0))
                   *ScSO3::exp(Vector4d(pi, 0, 0,0.0))
                   *ScSO3::exp(Vector4d(-0.2, -0.5, -0.0,0)));
  omegas.push_back(ScSO3::exp(Vector4d(0.3, 0.5, 0.1,0))
                   *ScSO3::exp(Vector4d(pi, 0, 0,0))
                   *ScSO3::exp(Vector4d(-0.3, -0.5, -0.1,0)));

  bool failed = false;

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d sR1 = omegas[i].matrix();
    Matrix3d sR2 = ScSO3::exp(omegas[i].log()).matrix();
    Matrix3d DiffR = sR1-sR2;
    double nrm = DiffR.norm();

    //// ToDO: Force ScSO3 to be more accurate!
    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "ScSO3 - exp(log(ScSO3))" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << sR1 << endl;
      cerr << omegas[i].log() << endl;
      cerr << sR2 << endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }

  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Vector3d p(1,2,4);
    Matrix3d sR = omegas[i].matrix();
    Vector3d res1 = omegas[i]*p;
    Vector3d res2 = sR*p;

    double nrm = (res1-res2).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Transform vector" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res1-res2) <<endl;
      cerr << endl;
      failed = true;
    }
  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d q = omegas[i].matrix();
    Matrix3d inv_q = omegas[i].inverse().matrix();
    Matrix3d res = q*inv_q ;
    Matrix3d I;
    I.setIdentity();

    double nrm = (res-I).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Inverse" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res-I) <<endl;
      cerr << endl;
      failed = true;
    }
    Matrix3d R = omegas[i].rotationMatrix();
    cerr << R*R.transpose() << endl;

  }
  return failed;
}

bool se2explog_tests()
{
  double pi = 3.14159265;
  vector<SE2> omegas;
  omegas.push_back(SE2(SO2(0.0),Vector2d(0,0)));
  omegas.push_back(SE2(SO2(0.2),Vector2d(10,0)));
  omegas.push_back(SE2(SO2(0.),Vector2d(0,100)));
  omegas.push_back(SE2(SO2(-1.),Vector2d(20,-1)));
  omegas.push_back(SE2(SO2(0.00001),Vector2d(-0.00000001,0.0000000001)));
  omegas.push_back(SE2(SO2(0.2),Vector2d(0,0))
                   *SE2(SO2(pi),Vector2d(0,0))
                   *SE2(SO2(-0.2),Vector2d(0,0)));
  omegas.push_back(SE2(SO2(0.3),Vector2d(2,0))
                   *SE2(SO2(pi),Vector2d(0,0))
                   *SE2(SO2(-0.3),Vector2d(0,6)));

  bool failed = false;

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d R1 = omegas[i].matrix();
    Matrix3d R2 = SE2::exp(omegas[i].log()).matrix();
    Matrix3d DiffR = R1-R2;
    double nrm = DiffR.norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "SE2 - exp(log(SE2))" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }
  }
  for (size_t i=0; i<omegas.size(); ++i)
  {
    Vector2d p(1,2);
    Matrix3d T = omegas[i].matrix();
    Vector2d res1 = omegas[i]*p;
    Vector2d res2 = T.topLeftCorner<2,2>()*p + T.topRightCorner<2,1>();

    double nrm = (res1-res2).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Transform vector" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res1-res2) <<endl;
      cerr << endl;
      failed = true;
    }
  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d q = omegas[i].matrix();
    Matrix3d inv_q = omegas[i].inverse().matrix();
    Matrix3d res = q*inv_q ;
    Matrix3d I;
    I.setIdentity();

    double nrm = (res-I).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Inverse" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res-I) <<endl;
      cerr << endl;
      failed = true;
    }
  }
  return failed;

}

bool so3explog_tests()
{

  vector<SO3> omegas;
  omegas.push_back(SO3(Quaterniond(0.1e-11, 0., 1., 0.)));
  omegas.push_back(SO3(Quaterniond(-1,0.00001,0.0,0.0)));
  omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0)));
  omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, -1.0)));
  omegas.push_back(SO3::exp(Vector3d(0., 0., 0.)));
  omegas.push_back(SO3::exp(Vector3d(0., 0., 0.00001)));
  omegas.push_back(SO3::exp(Vector3d(M_PI, 0, 0)));
  omegas.push_back(SO3::exp(Vector3d(0.2, 0.5, 0.0))
                   *SO3::exp(Vector3d(M_PI, 0, 0))
                   *SO3::exp(Vector3d(-0.2, -0.5, -0.0)));
  omegas.push_back(SO3::exp(Vector3d(0.3, 0.5, 0.1))
                   *SO3::exp(Vector3d(M_PI, 0, 0))
                   *SO3::exp(Vector3d(-0.3, -0.5, -0.1)));

  bool failed = false;

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d R1 = omegas[i].matrix();
    double theta;
    Matrix3d R2 = SO3::exp(SO3::logAndTheta(omegas[i],&theta)).matrix();

    Matrix3d DiffR = R1-R2;
    double nrm = DiffR.norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "SO3 - exp(log(SO3))" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << DiffR <<endl;
      cerr << endl;
      failed = true;
    }

    if (theta>M_PI || theta<-M_PI)
    {
      cerr << "log theta not in [-pi,pi]" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << theta <<endl;
      cerr << endl;
      failed = true;
    }

  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Vector3d p(1,2,4);
    Matrix3d sR = omegas[i].matrix();
    Vector3d res1 = omegas[i]*p;
    Vector3d res2 = sR*p;

    double nrm = (res1-res2).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Transform vector" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res1-res2) <<endl;
      cerr << endl;
      failed = true;
    }
  }

  for (size_t i=0; i<omegas.size(); ++i)
  {
    Matrix3d q = omegas[i].matrix();
    Matrix3d inv_q = omegas[i].inverse().matrix();
    Matrix3d res = q*inv_q ;
    Matrix3d I;
    I.setIdentity();

    double nrm = (res-I).norm();

    if (isnan(nrm) || nrm>SMALL_EPS)
    {
      cerr << "Inverse" << endl;
      cerr  << "Test case: " << i << endl;
      cerr << (res-I) <<endl;
      cerr << endl;
      failed = true;
    }
  }
  return failed;
}