Matrix<double,7,1> Sim3
::log(const Sim3& sim3)
{
Vector7d res;
double scale = sim3.scale();
Vector3d t = sim3.translation_;
double theta;
Vector4d omega_sigma = ScSO3::logAndTheta(sim3.scso3_, &theta);
Vector3d omega = omega_sigma.head<3>();
double sigma = omega_sigma[3];
Matrix3d Omega = SO3::hat(omega);
Matrix3d W = calcW(theta, sigma, scale, Omega);
//Vector3d upsilon = W.jacobiSvd(ComputeFullU | ComputeFullV).solve(t);
Vector3d upsilon = W.partialPivLu().solve(t);
res.segment(0,3) = upsilon;
res.segment(3,3) = omega;
res[6] = sigma;
return res;
}
Sim3 Sim3
::exp(const Matrix<double,7,1>& vect)
{
Vector3d upsilon = vect.segment(0,3);
Vector3d omega = vect.segment(3,3);
double sigma = vect[6];
double theta;
ScSO3 scso3 = ScSO3::expAndTheta(vect.tail<4>(), &theta);
Matrix3d Omega = SO3::hat(omega);
Matrix3d W = calcW(theta, sigma, scso3.scale(), Omega);
Vector3d t = W*upsilon;
return Sim3(scso3, t);
}
void CalcKinCharacteristics::run()
{
showParams();
core::Code code;
core::InternalGearRatios k;
core::GearSetTypes types;
core::FakeItem fake;
std::vector<core::IIOItem*> containers;
containers.push_back( &code );
containers.push_back( &k );
containers.push_back( &types );
containers.push_back( &fake );
while ( core::Singletons::getInstance()->getLoaderFromFile()->load( containers, core::IOFileManager::eOutputFileType::KIN_SLOW ) )
{
Characteristics ch;
ch._tooth = calcZ(k, types);
ch._torque = calcM( code, k );
ch._angVelocity = calcW( code, k, ch._tooth );
ch._power = calcN( ch._angVelocity, ch._torque );
ch._kpdZacStepen = calcKpdZacStepen( k, ch._angVelocity, ch._power );
ch._kpdTorque = calcMh( code, k, ch._kpdZacStepen );
ch._qualityCriterias = calcQualityCriterias( ch._kpdTorque, ch._angVelocity );
printCharacteristics( code, ch );
NS_CORE Singletons::getInstance()->getIOFileManager()->writeToFile( NS_CORE IOFileManager::eOutputFileType::RESULT, code );
NS_CORE GearBoxWithChanger gb( code );
gb.createChainsForAllgears();
NS_CORE Singletons::getInstance()->getIOFileManager()->writeToFile( NS_CORE IOFileManager::eOutputFileType::RESULT, gb );
NS_CORE Singletons::getInstance()->getIOFileManager()->writeToFile( NS_CORE IOFileManager::eOutputFileType::RESULT, types );
NS_CORE Singletons::getInstance()->getIOFileManager()->writeToFile( NS_CORE IOFileManager::eOutputFileType::RESULT, fake );
NS_CORE Singletons::getInstance()->getIOFileManager()->writeToFile( NS_CORE IOFileManager::eOutputFileType::RESULT, k );
NS_CORE Singletons::getInstance()->getIOFileManager()->writeToFile( NS_CORE IOFileManager::eOutputFileType::RESULT, ch );
NS_CORE Singletons::getInstance()->getIOFileManager()->writeToFile(NS_CORE IOFileManager::eOutputFileType::RESULT, NS_CORE IOFileManager::end);
m_characteristics.push_back( ch );
}
}
void MD5Anim::load(char *filename)
{
FILE *f;
char data[512];
f=fopen(filename,"rb");
int frameind;
float *frameData;
while(!feof(f))
{
fgets(data,sizeof(data),f);
if (sscanf(data," numFrames %d", &numFrames) == 1)
{
if (numFrames > 0)
{
bboxes=new Bounds[numFrames];
frameJoints=new Joint*[numFrames];
}
}
else if(sscanf(data," numJoints %d", &numJoints) == 1)
{
for(int i=0; i < numFrames; i++)
{
frameJoints[i] = new Joint[numJoints];
}
jointInfos=new Hierarchy[numJoints];
baseFrames=new BaseFrame[numJoints];
}
else if (sscanf(data," frameRate %d", &frameRate) == 1)
{
}
else if (sscanf(data," numAnimatedComponents %d", &numAnimComp) == 1)
{
if (numAnimComp > 0)
frameData=new float[numAnimComp];
}
else if (strncmp(data,"hierarchy {", 11) == 0)
{
for (int i=0; i < numJoints; i++)
{
fgets(data, sizeof(data),f);
sscanf(data, " %s %d %d %d", jointInfos[i].name,&jointInfos[i].parent, &jointInfos[i].flags,
&jointInfos[i].startIndex);
}
}
else if (strncmp(data,"bounds {", 8) == 0)
{
for (int i=0; i < numFrames; i++)
{
fgets(data,sizeof(data),f);
sscanf(data," ( %f %f %f ) ( %f %f %f )", &bboxes[i].min.x, &bboxes[i].min.y, &bboxes[i].min.z,
&bboxes[i].max.x, &bboxes[i].max.y, &bboxes[i].max.z);
}
}
else if (strncmp(data, "baseframe {", 11) == 0)
{
for (int i=0; i < numJoints; i++ )
{
fgets(data,sizeof(data),f);
if (sscanf(data," ( %f %f %f ) ( %f %f %f )", &baseFrames[i].translation.x, &baseFrames[i].translation.y,
&baseFrames[i].translation.z,&baseFrames[i].rotation.x,
&baseFrames[i].rotation.y, &baseFrames[i].rotation.z) == 6)
{
calcW(&baseFrames[i].rotation);
}
}
}
else if (sscanf( data, " frame %d", &frameind) == 1)
{
for (int i=0; i < numAnimComp; i++)
fscanf(f, "%f", &frameData[i]);
createFrameSkeleton(frameData,frameJoints[frameind]);
//build skeleton
}
}
fclose(f);
delete[] frameData;
}
void MD5Anim::createFrameSkeleton(float *frameData, Joint *skelFrame)
{
for (int i=0; i < numJoints; i++)
{
Vector3 animPos;
Quaternion animRot;
int k=0;
animPos=baseFrames[i].translation;
animRot=baseFrames[i].rotation;
if (jointInfos[i].flags & 1)
{
animPos.x=frameData[jointInfos[i].startIndex + k];
k++;
}
if (jointInfos[i].flags & 2)
{
animPos.y=frameData[jointInfos[i].startIndex + k];
k++;
}
if (jointInfos[i].flags & 4)
{
animPos.z=frameData[jointInfos[i].startIndex + k];
k++;
}
if (jointInfos[i].flags & 8)
{
animRot.x=frameData[jointInfos[i].startIndex + k];
k++;
}
if (jointInfos[i].flags & 16)
{
animRot.y=frameData[jointInfos[i].startIndex + k];
k++;
}
if (jointInfos[i].flags & 32)
{
animRot.z=frameData[jointInfos[i].startIndex + k];
k++;
}
calcW(&animRot);
Joint *curJoint=&skelFrame[i];
curJoint->parent=jointInfos[i].parent;
if (curJoint->parent < 0)
{
curJoint->translation=animPos;
curJoint->rotation=animRot;
}
else
{
Joint *parentJoint=&skelFrame[curJoint->parent];
Vector3 rotated=parentJoint->rotation.multiply3d(curJoint->translation);
curJoint->translation=parentJoint->translation + rotated;
curJoint->rotation=parentJoint->rotation*curJoint->rotation;
curJoint->rotation.normalize();
}
}
}
