inline static void setFromVectors(Rotation_& rot, const Eigen::Matrix<PrimType_, 3, 1>& v1, const Eigen::Matrix<PrimType_, 3, 1>& v2) {
KINDR_ASSERT_TRUE(std::runtime_error, v1.norm()*v2.norm() != static_cast<PrimType_>(0.0), "At least one vector has zero length.");
Eigen::Quaternion<PrimType_> eigenQuat;
eigenQuat.setFromTwoVectors(v1, v2);
rot = kindr::rotations::eigen_impl::RotationQuaternion<PrimType_, Usage_>(eigenQuat);
//
// const PrimType_ angle = acos(v1.dot(v2)/temp);
// const PrimType_ tol = 1e-3;
//
// if(0 <= angle && angle < tol) {
// rot.setIdentity();
// } else if(M_PI - tol < angle && angle < M_PI + tol) {
// rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, 1, 0, 0);
// } else {
// const Eigen::Matrix<PrimType_, 3, 1> axis = (v1.cross(v2)).normalized();
// rot = eigen_impl::AngleAxis<PrimType_, Usage_>(angle, axis);
// }
}
Quaternion<FloatT> exp(Eigen::Matrix<FloatT, 3, 1> v)
{
FloatT angle = v.norm();
if (angle <= Eigen::machine_epsilon<FloatT>()) {
// std::cerr << "Warning: tiny quaternion flushed to zero\n";
return Quaternion<FloatT>::Identity();
}
else {
Quaternion<FloatT> ret;
#if 0
if (angle > 1.999*M_PI) {
// TODO: I really, really don't like this hack. It should
// be impossible to compute an angular measurement update
// with a rotation angle greater than this number...
v *= 1.999*M_PI / angle;
angle = 1.999*M_PI;
}
#endif
assert(angle <= FloatT(2.0*M_PI));
#if 0
// Oddly enough, this attempt to make the formula faster by reducing
// the number of trig calls actually runs slower.
FloatT tan_x = std::tan(angle * 0.25);
FloatT cos_angle = (1 - tan_x*tan_x)/(1+tan_x*tan_x);
FloatT sin_angle = 2*tan_x/(1+tan_x*tan_x);
ret.w() = cos_angle;
ret.vec() = (sin_angle/angle)*v;
#else
ret.w() = std::cos(angle*0.5);
ret.vec() = (std::sin(angle*0.5)/angle)*v;
#endif
return ret;
// return Quaternion<FloatT>(Eigen::AngleAxis<FloatT>(angle, v / angle));
}
}
ReferenceSO3(const Eigen::Vector3d &v) : AbstractTestSO3(v) {
Eigen::Matrix<float_100, 3, 1> axis = v.cast<float_100>();
float_100 angle = axis.norm();
if (angle > 0) {
axis /= angle;
}
m_aa = Eigen::AngleAxis<float_100>(angle, axis);
}
void OpenGL3DSphereRenderer::saveTriangleInMem( const Eigen::Matrix<GLfloat,3,1>& v1, const Eigen::Matrix<GLfloat,3,1>& v2, const Eigen::Matrix<GLfloat,3,1>& v3, unsigned& current_vertex )
{
assert( fabs( double(v1.norm() - 1.0f) ) <= 1.0e-6 );
m_sphere_verts.col( current_vertex ) = v1;
m_sphere_normals.col( current_vertex ) = v1;
++current_vertex;
assert( fabs( double(v2.norm() - 1.0f) ) <= 1.0e-6 );
m_sphere_verts.col( current_vertex ) = v2;
m_sphere_normals.col( current_vertex ) = v2;
++current_vertex;
assert( fabs( double(v3.norm() - 1.0f) ) <= 1.0e-6 );
m_sphere_verts.col( current_vertex ) = v3;
m_sphere_normals.col( current_vertex ) = v3;
++current_vertex;
}
Eigen::Matrix<double,N,1> normalizeAndJacobian(const Eigen::Matrix<double,N,1> & v, Eigen::Matrix<double,N,N> & outJacobian)
{
double recip_s_vtv = 1.0/v.norm();
outJacobian = (Eigen::Matrix<double,N,N>::Identity() * recip_s_vtv) - (recip_s_vtv*recip_s_vtv*recip_s_vtv)*v*v.transpose();
return v*recip_s_vtv;
}
///
/// @par Detailed description
/// ...
/// @param [in, out] (param1) ...
/// @return ...
/// @note ...
float
sasmath::
calc_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3)
{
/*
Method to calculate the angle between three atoms
*/
float angle ;
Eigen::Matrix<float,3,1> u ; u = coor1 - coor2 ;
Eigen::Matrix<float,3,1> v ; v = coor3 - coor2 ;
float c = (u.dot(v))/(u.norm()*v.norm()) ;
angle = acos(c) ;
return angle ;
}
TEST_F(MaterialLibSolidsKelvinVector6, DeviatoricSphericalProjections)
{
auto const& P_dev = Invariants<size>::deviatoric_projection;
auto const& P_sph = Invariants<size>::spherical_projection;
// Test product P_dev * P_sph is zero.
Eigen::Matrix<double, 6, 6> const P_dev_P_sph = P_dev * P_sph;
EXPECT_LT(P_dev_P_sph.norm(), std::numeric_limits<double>::epsilon());
EXPECT_LT(P_dev_P_sph.maxCoeff(), std::numeric_limits<double>::epsilon());
// Test product P_sph * P_dev is zero.
Eigen::Matrix<double, 6, 6> const P_sph_P_dev = P_sph * P_dev;
EXPECT_LT(P_sph_P_dev.norm(), std::numeric_limits<double>::epsilon());
EXPECT_LT(P_sph_P_dev.maxCoeff(), std::numeric_limits<double>::epsilon());
// Test sum is identity.
EXPECT_EQ(P_dev + P_sph, (Eigen::Matrix<double, size, size>::Identity()));
}
Eigen::Matrix<double, 3, 3> getRotationFromA2B(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B) {
Eigen::Matrix<double, 3, 1> v = A.cross(B);
double s = v.norm();
double c = A.dot(B);
Eigen::Matrix<double, 3, 3> vx;
vx << 0, -v[2], v[1],
v[2], 0, -v[0],
-v[1], v[0], 0;
return Eigen::Matrix<double, 3, 3>::Identity() + vx + ((1.0-c)/(s*s))*vx*vx;
}
void singleArmTranslationConstraint:: constraintUpdate(const Eigen::Matrix<double, 3, 1> ¤tTranslation,
const Eigen::Matrix<double, 3, 1> &desiredTranslation,
const Eigen::Matrix<double, 3, 1> &timeDerivative,
const Eigen::Matrix<double, 6, DOF> &gradient
){
// update function measure
Eigen::Matrix<double, 3, 1> measure;
measure.setZero();
measure = currentTranslation - desiredTranslation;
measureNorm_ = measure.norm();
// update the constraint gradient
Eigen::Matrix<double, 3, DOF> tempGradient;
tempGradient.setZero();
tempGradient = gradient.block<3,DOF>(0,0);
// set up the gradient
for(int j = 0; j<dim_; j++){
for(int i = 0; i<DOF; i++){
// int k = i + sIndex_;
modelPointer_->chgCoeff(
*constrPArray_[j],
*(*jointVVarPArrayPointer_)[i],
tempGradient(j, i)
// gradient(j, i)
);
}// finish one constraint
}// finish all constraints
Eigen::Matrix<double, 3, 1> rhs;
rhs.setZero();
// update the rhs
for(int j = 0; j<dim_; j++){
rhs(j) = - gain_*( currentTranslation(j) - desiredTranslation(j) );
constrPArray_[j]->set(GRB_DoubleAttr_RHS,
rhs(j)
);
}
// std::cout<<"error X: "<<measure(0) <<" error Y: "<<measure(1) <<" error Z: "<<measure(2)<<std::endl;
// std::cout<<"error norm: "<<measureNorm_<<std::endl;
// std::cout<<"seperation line:---------------------------- "<<std::endl;
}
static bool testApply()
{
bool b, ret;
// apply delta:
Eigen::Matrix<double, 6, 1> delta = Eigen::Matrix<double, 6, 1>::Zero();
Eigen::Matrix<double, 4, 4> expectedT = Eigen::Matrix<double, 4, 4>::Identity();
Eigen::Matrix<double, 4, 4> diff;
SE3<double> pose;
pose.set( delta );
delta[ 0 ] = Math::deg2Rad( 1.5 );
delta[ 1 ] = Math::deg2Rad( 1.1 );
delta[ 2 ] = Math::deg2Rad( 1.6 );
delta[ 3 ] = 1;
delta[ 4 ] = 1;
delta[ 5 ] = 1;
pose.apply( delta );
expectedT( 0, 3 ) = delta[ 3 ];
expectedT( 1, 3 ) = delta[ 4 ];
expectedT( 2, 3 ) = delta[ 5 ];
Eigen::Matrix<double, 3, 1> axis = delta.segment<3>( 0 );
double angle = axis.norm(); axis /= angle;
expectedT.block<3, 3>( 0, 0 ) = Eigen::AngleAxis<double>( angle, axis ).toRotationMatrix();
diff = expectedT - pose.transformation();
ret = b = ( diff.array().abs().sum() / 12 < 0.001 );
if( !b ){
std::cout << expectedT << std::endl;
std::cout << pose.transformation() << std::endl;
std::cout << "avg SAD: " << diff.array().abs().sum() / 12 << std::endl;
}
pose.apply( -delta );
expectedT.setIdentity();
b &= ( ( expectedT - pose.transformation() ).array().abs().sum() / 12 < 0.0001 );
CVTTEST_PRINT( "apply", b );
ret &= b;
return ret;
}
static typename math::scalar_of<T>::type get(const Eigen::Matrix<T, N, M> &x) {
return x.norm();
}
double SolveODE(
std::list<double> &list_val,
std::list<Eigen::Matrix<double,Eigen::Dynamic,1> > &list_pos,
std::list<Eigen::Matrix<double,Eigen::Dynamic,1> > &list_vit,
std::list<Eigen::Matrix<double,Eigen::Dynamic,1> > &list_grad,
double &t_tot,
double &v_tot,
const std::vector<skeleton::BranchContProjSkel::Ptr> &skel,
const Eigen::Matrix<double,Eigen::Dynamic,1> &q_init,
const Eigen::Matrix<double,Eigen::Dynamic,1> &v_init,
const double &lambda,
const double &deltat)
{
double value = 0.0;
double valuef = 0.0;
Eigen::Matrix<double,Eigen::Dynamic,1> gradient(q_init.rows(),1);
Eigen::Matrix<double,Eigen::Dynamic,1> gradientf(q_init.rows(),1);
v_tot = 0.0;
t_tot = 0.0;
Dist_grad(q_init,skel,valuef,gradientf);
list_pos.push_back(q_init);
list_vit.push_back(v_init);
list_grad.push_back(gradientf);
list_val.push_back(valuef);
//first step, to avoid boundary problem :
Eigen::Matrix<double,Eigen::Dynamic,1> q = q_init + v_init * deltat;
Eigen::Matrix<double,Eigen::Dynamic,1> v = v_init;
gradient = gradientf;
value = valuef;
Dist_grad(q,skel,valuef,gradientf);
while(!Oob(q) && t_tot<10.0)
{
list_pos.push_back(q);
list_vit.push_back(v);
list_grad.push_back(gradientf);
list_val.push_back(valuef);
value = valuef;
gradient = gradientf;
Dist_grad(q,skel,valuef,gradientf);
double vnor = v.norm();
double frac = deltat/vnor;
v_tot+=(vnor*lambda + (value+valuef)/2.0)*frac;
t_tot += frac;
Eigen::Matrix<double,Eigen::Dynamic,1> acc = gradientf*(1.0/lambda);
q = q + v*frac;
v = v + acc*frac;
}
Eigen::Matrix<double,Eigen::Dynamic,1> qf = Eigen::Matrix<double,Eigen::Dynamic,1>::Ones(q.rows(),1);
value = valuef;
gradient = gradientf;
Dist_grad(qf,skel,valuef,gradientf);
double df = (q-qf).norm();
v_tot+=df*lambda + ((valuef+value)/2.0)*df;
list_pos.push_back(qf);
list_vit.push_back(v);
list_grad.push_back(gradientf);
list_val.push_back(valuef);
return df;
}
Eigen::Matrix<double,N,1> normalize(const Eigen::Matrix<double,N,1> & v)
{
return v/v.norm();
}