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