void IKTree::reset(int frame)
{
MT_Quaternion q;
float rad;
BVHNode *n;
for (int i=0; i<numBones; i++) {
bone[i].pos = origin;
bone[i].lRot = identity;
bone[i].gRot = identity;
n = bone[i].node;
for (int k=0; k<3; k++) { // rotate each axis in order
rad = n->frame[frame][k] * M_PI / 180;
q = identity;
switch (n->channelType[k]) {
case BVH_XROT: q.setRotation(xAxis, rad); break;
case BVH_YROT: q.setRotation(yAxis, rad); break;
case BVH_ZROT: q.setRotation(zAxis, rad); break;
case BVH_XPOS: bone[i].pos[0] = n->frame[frame][k]; break;
case BVH_YPOS: bone[i].pos[1] = n->frame[frame][k]; break;
case BVH_ZPOS: bone[i].pos[2] = n->frame[frame][k]; break;
}
bone[i].lRot = q * bone[i].lRot;
}
}
updateBones(0);
}
void IKTree::reset(int frame)
{
MT_Quaternion q;
BVHNode *n;
for (int i=0; i<numBones; i++) {
bone[i].pos = origin;
bone[i].lRot = identity;
bone[i].gRot = identity;
n = bone[i].node;
Rotation rot=n->frameData(frame).rotation();
Position pos=n->frameData(frame).position();
for (int k=0; k<n->numChannels; k++) { // rotate each axis in order
q = identity;
switch (n->channelType[k]) {
case BVH_XROT:
q.setRotation(xAxis, rot.x * M_PI / 180);
break;
case BVH_YROT:
q.setRotation(yAxis, rot.y * M_PI / 180);
break;
case BVH_ZROT:
q.setRotation(zAxis, rot.z * M_PI / 180);
break;
case BVH_XPOS: bone[i].pos[0] = pos.x; break;
case BVH_YPOS: bone[i].pos[1] = pos.y; break;
case BVH_ZPOS: bone[i].pos[2] = pos.z; break;
}
bone[i].lRot = q * bone[i].lRot;
}
/*
for (int k=0; k<3; k++) { // rotate each axis in order
rad = n->frame[frame][k] * M_PI / 180;
q = identity;
switch (n->channelType[k]) {
case BVH_XROT: q.setRotation(xAxis, rad); break;
case BVH_YROT: q.setRotation(yAxis, rad); break;
case BVH_ZROT: q.setRotation(zAxis, rad); break;
case BVH_XPOS: bone[i].pos[0] = n->frame[frame][k]; break;
case BVH_YPOS: bone[i].pos[1] = n->frame[frame][k]; break;
case BVH_ZPOS: bone[i].pos[2] = n->frame[frame][k]; break;
}
bone[i].lRot = q * bone[i].lRot;
}
*/
}
updateBones(0);
}
void
MT_ExpMap::
setRotation(
const MT_Quaternion &q
) {
// ok first normalize the quaternion
// then compute theta the axis-angle and the normalized axis v
// scale v by theta and that's it hopefully!
m_q = q.normalized();
m_v = MT_Vector3(m_q.x(), m_q.y(), m_q.z());
MT_Scalar cosp = m_q.w();
m_sinp = m_v.length();
m_v /= m_sinp;
m_theta = atan2(double(m_sinp),double(cosp));
m_v *= m_theta;
}
void
MT_ExpMap::
compute_dRdVi(
const MT_Quaternion &dQdvi,
MT_Matrix3x3 & dRdvi
) const {
MT_Scalar prod[9];
/* This efficient formulation is arrived at by writing out the
* entire chain rule product dRdq * dqdv in terms of 'q' and
* noticing that all the entries are formed from sums of just
* nine products of 'q' and 'dqdv' */
prod[0] = -MT_Scalar(4)*m_q.x()*dQdvi.x();
prod[1] = -MT_Scalar(4)*m_q.y()*dQdvi.y();
prod[2] = -MT_Scalar(4)*m_q.z()*dQdvi.z();
prod[3] = MT_Scalar(2)*(m_q.y()*dQdvi.x() + m_q.x()*dQdvi.y());
prod[4] = MT_Scalar(2)*(m_q.w()*dQdvi.z() + m_q.z()*dQdvi.w());
prod[5] = MT_Scalar(2)*(m_q.z()*dQdvi.x() + m_q.x()*dQdvi.z());
prod[6] = MT_Scalar(2)*(m_q.w()*dQdvi.y() + m_q.y()*dQdvi.w());
prod[7] = MT_Scalar(2)*(m_q.z()*dQdvi.y() + m_q.y()*dQdvi.z());
prod[8] = MT_Scalar(2)*(m_q.w()*dQdvi.x() + m_q.x()*dQdvi.w());
/* first row, followed by second and third */
dRdvi[0][0] = prod[1] + prod[2];
dRdvi[0][1] = prod[3] - prod[4];
dRdvi[0][2] = prod[5] + prod[6];
dRdvi[1][0] = prod[3] + prod[4];
dRdvi[1][1] = prod[0] + prod[2];
dRdvi[1][2] = prod[7] - prod[8];
dRdvi[2][0] = prod[5] - prod[6];
dRdvi[2][1] = prod[7] + prod[8];
dRdvi[2][2] = prod[0] + prod[1];
}
int
main(int argc, char **argv)
{
const int seg_num = 5;
const MT_Scalar seg_length = 15;
const float seg_startA[3] = {0,0,0};
const float seg_startB[3] = {0,-20,0};
// create some segments to solve with
// First chain
//////////////
IK_Segment_ExternPtr const segmentsA = new IK_Segment_Extern[seg_num];
IK_Segment_ExternPtr const segmentsB = new IK_Segment_Extern[seg_num];
IK_Segment_ExternPtr seg_it = segmentsA;
IK_Segment_ExternPtr seg_itB = segmentsB;
{
// MT_Quaternion qmat(MT_Vector3(0,0,1),-3.141/2);
MT_Quaternion qmat(MT_Vector3(0,0,1),0);
MT_Matrix3x3 mat(qmat);
seg_it->seg_start[0] = seg_startA[0];
seg_it->seg_start[1] = seg_startA[1];
seg_it->seg_start[2] = seg_startA[2];
float temp[12];
mat.getValue(temp);
seg_it->basis[0] = temp[0];
seg_it->basis[1] = temp[1];
seg_it->basis[2] = temp[2];
seg_it->basis[3] = temp[4];
seg_it->basis[4] = temp[5];
seg_it->basis[5] = temp[6];
seg_it->basis[6] = temp[8];
seg_it->basis[7] = temp[9];
seg_it->basis[8] = temp[10];
seg_it->length = seg_length;
MT_Quaternion q;
q.setEuler(0,0,0);
MT_Matrix3x3 qrot(q);
seg_it->basis_change[0] = 1;
seg_it->basis_change[1] = 0;
seg_it->basis_change[2] = 0;
seg_it->basis_change[3] = 0;
seg_it->basis_change[4] = 1;
seg_it->basis_change[5] = 0;
seg_it->basis_change[6] = 0;
seg_it->basis_change[7] = 0;
seg_it->basis_change[8] = 1;
seg_it ++;
seg_itB->seg_start[0] = seg_startA[0];
seg_itB->seg_start[1] = seg_startA[1];
seg_itB->seg_start[2] = seg_startA[2];
seg_itB->basis[0] = temp[0];
seg_itB->basis[1] = temp[1];
seg_itB->basis[2] = temp[2];
seg_itB->basis[3] = temp[4];
seg_itB->basis[4] = temp[5];
seg_itB->basis[5] = temp[6];
seg_itB->basis[6] = temp[8];
seg_itB->basis[7] = temp[9];
seg_itB->basis[8] = temp[10];
seg_itB->length = seg_length;
seg_itB->basis_change[0] = 1;
seg_itB->basis_change[1] = 0;
seg_itB->basis_change[2] = 0;
seg_itB->basis_change[3] = 0;
seg_itB->basis_change[4] = 1;
seg_itB->basis_change[5] = 0;
seg_itB->basis_change[6] = 0;
seg_itB->basis_change[7] = 0;
seg_itB->basis_change[8] = 1;
seg_itB ++;
}
int i;
for (i=1; i < seg_num; ++i, ++seg_it,++seg_itB) {
MT_Quaternion qmat(MT_Vector3(0,0,1),0.3);
MT_Matrix3x3 mat(qmat);
seg_it->seg_start[0] = 0;
seg_it->seg_start[1] = 0;
seg_it->seg_start[2] = 0;
float temp[12];
mat.getValue(temp);
seg_it->basis[0] = temp[0];
seg_it->basis[1] = temp[1];
seg_it->basis[2] = temp[2];
seg_it->basis[3] = temp[4];
seg_it->basis[4] = temp[5];
seg_it->basis[5] = temp[6];
seg_it->basis[6] = temp[8];
seg_it->basis[7] = temp[9];
seg_it->basis[8] = temp[10];
seg_it->length = seg_length;
MT_Quaternion q;
q.setEuler(0,0,0);
MT_Matrix3x3 qrot(q);
seg_it->basis_change[0] = 1;
seg_it->basis_change[1] = 0;
seg_it->basis_change[2] = 0;
seg_it->basis_change[3] = 0;
seg_it->basis_change[4] = 1;
seg_it->basis_change[5] = 0;
seg_it->basis_change[6] = 0;
seg_it->basis_change[7] = 0;
seg_it->basis_change[8] = 1;
///////////////////////////////
seg_itB->seg_start[0] = 0;
seg_itB->seg_start[1] = 0;
seg_itB->seg_start[2] = 0;
seg_itB->basis[0] = temp[0];
seg_itB->basis[1] = temp[1];
seg_itB->basis[2] = temp[2];
seg_itB->basis[3] = temp[4];
seg_itB->basis[4] = temp[5];
seg_itB->basis[5] = temp[6];
seg_itB->basis[6] = temp[8];
seg_itB->basis[7] = temp[9];
seg_itB->basis[8] = temp[10];
seg_itB->length = seg_length;
seg_itB->basis_change[0] = 1;
seg_itB->basis_change[1] = 0;
seg_itB->basis_change[2] = 0;
seg_itB->basis_change[3] = 0;
seg_itB->basis_change[4] = 1;
seg_itB->basis_change[5] = 0;
seg_itB->basis_change[6] = 0;
seg_itB->basis_change[7] = 0;
seg_itB->basis_change[8] = 1;
}
// create the chains
const int num_chains = 2;
IK_Chain_ExternPtr chains[num_chains];
chains[0] = IK_CreateChain();
chains[1] = IK_CreateChain();
// load segments into chain
IK_LoadChain(chains[0],segmentsA,seg_num);
IK_LoadChain(chains[1],segmentsB,seg_num);
// make and install a mouse handler
MEM_SmartPtr<MyGlutMouseHandler> mouse_handler (MyGlutMouseHandler::New());
GlutMouseManager::Instance()->InstallHandler(mouse_handler);
mouse_handler->SetChain(chains,num_chains);
// make and install a keyhandler
MEM_SmartPtr<MyGlutKeyHandler> key_handler (MyGlutKeyHandler::New());
GlutKeyboardManager::Instance()->InstallHandler(key_handler);
// instantiate the drawing class
MEM_SmartPtr<ChainDrawer> drawer (ChainDrawer::New());
GlutDrawManager::Instance()->InstallDrawer(drawer);
drawer->SetMouseHandler(mouse_handler);
drawer->SetChain(chains,num_chains);
drawer->SetKeyHandler(key_handler);
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB | GLUT_DEPTH);
glutCreateWindow("ik");
glutDisplayFunc(GlutDrawManager::Draw);
glutMouseFunc(GlutMouseManager::Mouse);
glutMotionFunc(GlutMouseManager::Motion);
glutKeyboardFunc(GlutKeyboardManager::HandleKeyboard);
init(MT_Vector3(-50,-50,-50),MT_Vector3(50,50,50));
glutMainLoop();
return 0; /* ANSI C requires main to return int. */
}
void IKTree::solveJoint(int frame, int i, IKEffectorList &effList)
{
double x, y, z;
double ang = 0;
MT_Quaternion q;
MT_Quaternion totalPosRot = MT_Quaternion(0,0,0,0);
MT_Quaternion totalDirRot = MT_Quaternion(0,0,0,0);
MT_Vector3 axis(0,0,0);
BVHNode *n;
int numPosRot = 0, numDirRot = 0;
if (bone[i].numChildren == 0) { // reached end site
if (bone[i].node->ikOn) {
effList.index[effList.num++] = i;
}
return;
}
for (int j=0; j<bone[i].numChildren; j++) {
IKEffectorList el;
el.num = 0;
solveJoint(frame, bone[i].child[j], el);
for (int k=0; k<el.num; k++) {
effList.index[effList.num++] = el.index[k];
}
}
updateBones(i);
for (int j=0; j<effList.num; j++) {
int effIndex = effList.index[j];
n = bone[effIndex].node;
MT_Vector3 effGoalPos(n->ikGoalPos[0],
n->ikGoalPos[1],
n->ikGoalPos[2]);
const MT_Vector3 pC =
(bone[effIndex].pos - bone[i].pos).safe_normalized();
const MT_Vector3 pD =
(effGoalPos - bone[i].pos).safe_normalized();
MT_Vector3 rotAxis = pC.cross(pD);
if (rotAxis.length2() > MT_EPSILON) {
totalPosRot += MT_Quaternion(rotAxis, bone[i].weight * acos(pC.dot(pD)));
numPosRot++;
}
const MT_Vector3 uC =
(bone[effIndex].pos - bone[effIndex-1].pos).safe_normalized();
const MT_Vector3 uD =
(MT_Vector3(n->ikGoalDir[0], n->ikGoalDir[1], n->ikGoalDir[2])).safe_normalized();
rotAxis = uC.cross(uD);
if (rotAxis.length2() > MT_EPSILON) {
double weight = 0.0;
if (i == effIndex-1) weight = 0.5;
totalDirRot += MT_Quaternion(rotAxis, weight * acos(uC.dot(uD)));
numDirRot++;
}
}
if ((numPosRot + numDirRot) > MT_EPSILON) {
n = bone[i].node;
n->ikOn = true;
// average the quaternions from all effectors
if (numPosRot)
totalPosRot /= numPosRot;
else
totalPosRot = identity;
if (numDirRot)
totalDirRot /= numDirRot;
else
totalDirRot = identity;
MT_Quaternion targetRot = 0.9 * totalPosRot + 0.1 * totalDirRot;
targetRot = targetRot * bone[i].lRot;
toEuler(targetRot, n->channelOrder, x, y, z);
if (jointLimits) {
bone[i].lRot = identity;
for (int k=0; k<n->numChannels; k++) { // clamp each axis in order
switch (n->channelType[k]) {
case BVH_XROT: ang = x; axis = xAxis; break;
case BVH_YROT: ang = y; axis = yAxis; break;
case BVH_ZROT: ang = z; axis = zAxis; break;
default: break;
}
// null axis leads to crash in q.setRotation(), so check first
if(axis.length())
{
if (ang < n->channelMin[k]) ang = n->channelMin[k];
else if (ang > n->channelMax[k]) ang = n->channelMax[k];
q.setRotation(axis, ang * M_PI / 180);
bone[i].lRot = q * bone[i].lRot;
}
}
}
else
bone[i].lRot = targetRot;
}
}
