Eigen::Matrix<T,4,1> blend(T t, Eigen::Matrix<T,4,1> p00, Eigen::Matrix<T,4,1> pNorm,
Eigen::Matrix<T,4,1> p10){
T r0 = p00.mag();
T r1 = p10.mag();
T rs = linear(t,r0,r1);
T dotp = dot(p00,p10);
dotp = abs(dotp);
T theta = acos(dotp/r0/r1);
Eigen::Quaternion<T> dq0;
p00.normalize();
p10.normalize();
Eigen::Quaternion<T> q00(p00);
Eigen::Quaternion<T> q10(p10);
if (theta < 0.01) {
//just give it a little nudge in the right direction
T o = 0.10;
Eigen::Quaternion<T> dq(cos(o/2.), sin(o/2.)*pNorm[0], sin(o/2.)*pNorm[1], sin(o/2.)*pNorm[2]);
Eigen::Matrix<T,4,1> p01 = dq.rotate(p00);
Eigen::Quaternion<T> q01(p01);
dq0 = Eigen::Quaternion<T>::slerp(q01,q10,t);
}
else {
dq0 = Eigen::Quaternion<T>::slerp(q00,q10,t);
}
T dott = p00.dot(p10);
T dotq0 = q00.dot(q10);
T dotq1 = dq0.dot(q10);
return Eigen::Matrix<T,4,1>(rs*dq0[1],rs*dq0[2],rs*dq0[3]);
//return Eigen::Matrix<T,4,1>(p01);
}
void QuatClassTest::testQuatClassTestCreation()
{
// test that it initializes to the multiplication identity
gmtl::Quat<float> q;
CPPUNIT_ASSERT( q[gmtl::Xelt] == 0.0f );
CPPUNIT_ASSERT( q[gmtl::Yelt] == 0.0f );
CPPUNIT_ASSERT( q[gmtl::Zelt] == 0.0f );
CPPUNIT_ASSERT( q[gmtl::Welt] == 1.0f );
// try out get...
float x = 102, y = 103, z = 101, w = 100;
q.get( x, y, z, w );
CPPUNIT_ASSERT( x == 0.0f );
CPPUNIT_ASSERT( y == 0.0f );
CPPUNIT_ASSERT( z == 0.0f );
CPPUNIT_ASSERT( w == 1.0f );
// try out set...
q.set( 1, 2, 3, 4 );
CPPUNIT_ASSERT( q[gmtl::Xelt] == 1.0f );
CPPUNIT_ASSERT( q[gmtl::Yelt] == 2.0f );
CPPUNIT_ASSERT( q[gmtl::Zelt] == 3.0f );
CPPUNIT_ASSERT( q[gmtl::Welt] == 4.0f );
// try out setting with brackets
q[gmtl::Xelt] = 5;
q[gmtl::Yelt] = 6;
q[gmtl::Zelt] = 7;
q[gmtl::Welt] = 8;
CPPUNIT_ASSERT( q[gmtl::Xelt] == 5.0f );
CPPUNIT_ASSERT( q[gmtl::Yelt] == 6.0f );
CPPUNIT_ASSERT( q[gmtl::Zelt] == 7.0f );
CPPUNIT_ASSERT( q[gmtl::Welt] == 8.0f );
q.get( x, y, z, w );
CPPUNIT_ASSERT( x == 5.0f );
CPPUNIT_ASSERT( y == 6.0f );
CPPUNIT_ASSERT( z == 7.0f );
CPPUNIT_ASSERT( w == 8.0f );
// try out element constructor
gmtl::Quat<float> q2( 10, 11, 12, 13 );
CPPUNIT_ASSERT( q2[gmtl::Xelt] == 10.0f );
CPPUNIT_ASSERT( q2[gmtl::Yelt] == 11.0f );
CPPUNIT_ASSERT( q2[gmtl::Zelt] == 12.0f );
CPPUNIT_ASSERT( q2[gmtl::Welt] == 13.0f );
// try out copy constructor
gmtl::Quat<float> q3( q );
CPPUNIT_ASSERT( q3[gmtl::Xelt] == 5.0f );
CPPUNIT_ASSERT( q3[gmtl::Yelt] == 6.0f );
CPPUNIT_ASSERT( q3[gmtl::Zelt] == 7.0f );
CPPUNIT_ASSERT( q3[gmtl::Welt] == 8.0f );
// try out operator=() function
gmtl::Quat<float> q4;
CPPUNIT_ASSERT( q4[gmtl::Xelt] == 0.0f );
CPPUNIT_ASSERT( q4[gmtl::Yelt] == 0.0f );
CPPUNIT_ASSERT( q4[gmtl::Zelt] == 0.0f );
CPPUNIT_ASSERT( q4[gmtl::Welt] == 1.0f );
q4 = q2;
CPPUNIT_ASSERT( q4[gmtl::Xelt] == 10.0f );
CPPUNIT_ASSERT( q4[gmtl::Yelt] == 11.0f );
CPPUNIT_ASSERT( q4[gmtl::Zelt] == 12.0f );
CPPUNIT_ASSERT( q4[gmtl::Welt] == 13.0f );
// check out the const identities...
gmtl::Quat<float> q9( gmtl::QUAT_IDENTITYF );
CPPUNIT_ASSERT( q9[gmtl::Xelt] == 0.0f );
CPPUNIT_ASSERT( q9[gmtl::Yelt] == 0.0f );
CPPUNIT_ASSERT( q9[gmtl::Zelt] == 0.0f );
CPPUNIT_ASSERT( q9[gmtl::Welt] == 1.0f );
gmtl::Quat<float> q10( gmtl::QUAT_MULT_IDENTITYF );
CPPUNIT_ASSERT( q10[gmtl::Xelt] == 0.0f );
CPPUNIT_ASSERT( q10[gmtl::Yelt] == 0.0f );
CPPUNIT_ASSERT( q10[gmtl::Zelt] == 0.0f );
CPPUNIT_ASSERT( q10[gmtl::Welt] == 1.0f );
gmtl::Quat<float> q11( gmtl::QUAT_ADD_IDENTITYF );
CPPUNIT_ASSERT( q11[gmtl::Xelt] == 0.0f );
CPPUNIT_ASSERT( q11[gmtl::Yelt] == 0.0f );
CPPUNIT_ASSERT( q11[gmtl::Zelt] == 0.0f );
CPPUNIT_ASSERT( q11[gmtl::Welt] == 0.0f );
}
/*Build the MBTB_body and set to the initial postion.*/
void MBTB_BodyBuild(unsigned int numDS, const std::string& BodyName, double mass,
SP::SiconosVector initPos, SP::SiconosVector modelCenterMass,
SP::SimpleMatrix inertialMatrix,
const std::string& pluginFextLib, const std::string& pluginFextFct,
const std::string& pluginMextLib, const std::string& pluginMextFct,
const std::string& pluginFintLib, const std::string& pluginFintFct,
const std::string& pluginMintLib, const std::string& pluginMintFct,
const std::string& pluginFintJacqLib, const std::string& pluginFintJacqFct,
const std::string& pluginMintJacqLib, const std::string& pluginMintJacqFct,
const std::string& pluginFintJacvLib, const std::string& pluginFintJacvFct,
const std::string& pluginMintJacvLib, const std::string& pluginMintJacvFct,
const std::string& pluginBoundaryConditionLib, const std::string& pluginBoundaryConditionFct,
SP::IndexInt boundaryConditionIndex)
{
assert(sNbOfBodies > numDS &&"MBTB_BodyBuild numDS out of range.");
unsigned int qDim=7;
//unsigned int nDof = 3;
unsigned int nDim = 6;
SP::SiconosVector q10(new SiconosVector(qDim));
SP::SiconosVector v10(new SiconosVector(nDim));
_MBTB_BodyBuildComputeInitPosition(numDS,mass,initPos,modelCenterMass,inertialMatrix,q10,v10);
MBTB_Body * p=0;
p =new MBTB_Body(q10,v10,mass,inertialMatrix,modelCenterMass,
BodyName, BodyName);
// We fix a ds number just to be able to use postprocessing based on hdf5 file
p->setNumber(numDS+1);
// set external forces plugin
if(pluginFextFct.length()>1)
{
p->setComputeFExtFunction(pluginFextLib,pluginFextFct);
}
if(pluginMextFct.length()>1)
{
p->setComputeMExtFunction(pluginMextLib,pluginMextFct);
}
// set internal forces plugin
if(pluginFintFct.length()>1)
{
p->setComputeFIntFunction(pluginFintLib,pluginFintFct);
if(pluginFintJacqFct.length()>1)
{
if (pluginFintJacqFct == "FiniteDifference")
{
std::cout <<"setComputeJacobianFIntqByFD(true)" <<std::endl;
p->setComputeJacobianFIntqByFD(true);
}
else
{
p->setComputeJacobianFIntqFunction(pluginFintJacqLib,pluginFintJacqFct);
}
}
if(pluginFintJacvFct.length()>1)
{
if (pluginFintJacvFct == "FiniteDifference")
{
std::cout <<"setComputeJacobianFIntvByFD(true)" <<std::endl;
p->setComputeJacobianFIntvByFD(true);
}
else
{
p->setComputeJacobianFIntvFunction(pluginFintJacvLib,pluginFintJacvFct);
}
}
}
if(pluginMintFct.length()>1)
{
p->setComputeMIntFunction(pluginMintLib,pluginMintFct);
if(pluginMintJacqFct.length()>1)
{
if (pluginMintJacqFct == "FiniteDifference")
{
std::cout <<"setComputeJacobianMIntqByFD(true)" <<std::endl;
p->setComputeJacobianMIntqByFD(true);
}
else
{
p->setComputeJacobianMIntqFunction(pluginMintJacqLib,pluginMintJacqFct);
}
}
if(pluginMintJacvFct.length()>1)
{
if (pluginMintJacvFct == "FiniteDifference")
{
std::cout <<"setComputeJacobianMIntvByFD(true)" <<std::endl;
p->setComputeJacobianMIntvByFD(true);
}
else
{
p->setComputeJacobianMIntvFunction(pluginMintJacvLib,pluginMintJacvFct);
}
}
}
// set boundary condition
if (pluginBoundaryConditionFct.length() >1)
{
//SP::IndexInt bdindex(new IndexInt(1));
//(*bdindex)[0] = 4;
DEBUG_PRINT("################################################################\n");
DEBUG_PRINT("###\n");
DEBUG_PRINT("###\n");
DEBUG_PRINT("###\n");
DEBUG_PRINTF("Set boundary Condition for body numDs = %i\n", numDS);
DEBUG_EXPR(
for (std::vector<unsigned int>::iterator itindex = boundaryConditionIndex->begin() ;
itindex != boundaryConditionIndex->end();
++itindex)
{std::cout << *itindex <<std::endl;};
);
