www.pudn.com > 人体步态跟踪识别bate版.rar > Walker.cpp
// Walker.cpp: implementation of the CWalker class.
//
//////////////////////////////////////////////////////////////////////
#include "stdafx.h"
#include "humantrack.h"
#include "Walker.h"
#include "IK.h"
#include "math.h"
#include "dibapi.h"
#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif
//////////////////////////////////////////////////////////////////////
// Construction/Destruction
//////////////////////////////////////////////////////////////////////
CWalker::CWalker()
{
double len_thigh = 45; // the length of thigh
double len_calf = 40; // the length of calf
double len_foot = 18; // the length of foot
double ulimit_hip = 2*PI; // the maximum value for the rotation of hip
double llimit_hip = PI; // the minimum value for the rotation of hip
double ulimit_knee = 2*PI; // the maximum value for the rotation of knee
double llimit_knee = PI; // the minimum value for the rotation of knee
double ulimit_ankle = 2*PI; // the maximum value for the rotation of ankle
double llimit_ankle = PI; // the minimum value for the rotation of ankle
/*** Control Variables*/
bool IsLeftLegFront = true; // true -> LeftLeg is front leg
}
CWalker::~CWalker()
{
}
CWalker::CWalker(double len_thigh1,double len_calf1,double len_foot1)
{
len_thigh = len_thigh1;
len_calf = len_calf1;
len_foot = len_foot1;
}
CWalker::CWalker(double len_thigh1,double len_calf1,double len_foot1,double ulimit_hip1,double llimit_hip1,
double ulimit_knee1,double llimit_knee1,double ulimit_ankle1,double llimit_ankle1 )
{
len_thigh = len_thigh1;
len_calf = len_calf1;
len_foot = len_foot1;
ulimit_hip = ulimit_hip1;
llimit_hip = llimit_hip1;
ulimit_knee = ulimit_knee1;
llimit_knee = llimit_knee1;
ulimit_ankle = ulimit_ankle1;
llimit_ankle = llimit_ankle1;
}
/**
* Set segment length
*/
void CWalker::setLenLeg(double len_thigh1,double len_calf1,double len_foot1 )
{
len_thigh = len_thigh1;
len_calf = len_calf1;
len_foot = len_foot1;
}
/**
* Set joint limit of hip
*/
void CWalker::setLimitRotaHip(double ulimit_hip1,double llimit_hip1 )
{
ulimit_hip = ulimit_hip1;
llimit_hip = llimit_hip1;
}
/**
* Set joint limit of knee
*/
void CWalker::setLimitRotaKnee(double ulimit_knee1,double llimit_knee1 )
{
ulimit_knee = ulimit_knee1;
llimit_knee = llimit_knee1;
}
/**
* Set joint limit of ankle
*/
void CWalker::setLimitRotaAnkle(double ulimit_ankle1,double llimit_ankle1 )
{
ulimit_ankle = ulimit_ankle;
llimit_ankle = llimit_ankle;
}
/**
* Query the walk configuration, if failure
* or no more frames available, return null
*/
CWalkConf CWalker::query(CPosition from, CPosition to,CPosition next, int whichFrame)
{
CWalkConf cf;
if(from.equals(preFrom)==false || to.equals(preTo)==false)
{
// if walk false, then return null
wa = walk(from, to, next);
if(wa->IsEmpty())
return cf;
}
// if no more frames available, then return null
if(whichFrame >= wa->length)
return cf;
return *(wa+whichFrame);
}
CWalkConf CWalker::query(CPosition to, CPosition next,int whichSlice )
{
return query(preTo, to, next, whichSlice);
}
CWalkConf* CWalker::walk(CPosition from, CPosition to, CPosition next)
{
CWalkConf* cf1=new CWalkConf;
// check the input
if(from.IsEmpty() || to.IsEmpty())
return cf1;
int numKeyframe = 4;
// generate 4 keyframes
CWalkConf keyframe1 = genKeyframe1(from, to);
CWalkConf keyframe2 = genKeyframe2(from, to);
CWalkConf keyframe3 = genKeyframe3(from, to);
CWalkConf keyframe4 = genKeyframe4(to, next);
// if some keyframes fail to generate
if(keyframe1.IsEmpty()|| keyframe2.IsEmpty() ||keyframe3.IsEmpty()|| keyframe4.IsEmpty())
{
wa = new CWalkConf[1];
// if some keyframes fail to generate, then skip all
// frames and just generate Standing Configuration in to
*(wa+0) = genStandConf(to);
}
else
{
// use specil functions to interpolate between all keyframes
CWalkConf * wa1 = genInterpolation1(keyframe1, keyframe2);
CWalkConf * wa2 = genInterpolation2(keyframe2, keyframe3);
CWalkConf * wa3 = genInterpolation3(keyframe3, keyframe4);
numKeyframe = numKeyframe + wa1->length +wa2->length + wa3->length;
int start = 0;
if(preFrom.IsEmpty())
{
start = 1;
numKeyframe++;
}
if(next.IsEmpty()) numKeyframe++;
wa = new CWalkConf[numKeyframe];
// if it is the first time to plan Walk, then generate
// Standing Configuration in from
if(preFrom.IsEmpty()) wa[0] = genStandConf(from);
*(wa+start++) = keyframe1;
for(int j = 0; j < wa1->length; j++) wa[start++] = wa1[j];
*(wa+start++) = keyframe2;
for(j = 0; j < wa2->length; j++) wa[start++] = wa2[j];
*(wa+start++) = keyframe3;
for(j = 0; j < wa3->length; j++) wa[start++] = wa3[j];
*(wa+start++) = keyframe4;
// if there is no any step to walk, then generate
// Standing Configuration in to
if(next.IsEmpty()) *(wa+start) = genStandConf(to);
// judge if it needs to exchange configurations of two leg
if(!IsLeftLegFront)
for(int i = 0; i < wa->length; i++) (wa+i)->exchange();
}
// modify the information
preFrom = from;
preTo = to;
IsLeftLegFront = !IsLeftLegFront;
return wa;
}
CWalkConf CWalker::genKeyframe1(CPosition from, CPosition to)
{
CWalkConf cf;
if(from.IsEmpty() || to.IsEmpty())
return cf;
// put Keyframe-Generating function you want here :
return AKeyframe(from, to);
}
CWalkConf CWalker::genKeyframe2(CPosition from, CPosition to)
{
CWalkConf cf;
if(from.IsEmpty() || to.IsEmpty())
return cf;
// put Keyframe-Generating function you want here :
return BKeyframe(from, to);
}
CWalkConf CWalker::genKeyframe3(CPosition from, CPosition to)
{
CWalkConf cf;
if(from.IsEmpty() || to.IsEmpty())
return cf;
// put Keyframe-Generating function you want here :
return CKeyframe(from, to);
}
CWalkConf CWalker::genKeyframe4(CPosition to, CPosition next)
{
CWalkConf cf;
if(to.IsEmpty())
return cf;
// put Keyframe-Generating function you want here :
return AKeyframe(to, next);
}
/**
* Interpolate Keyframe 1 and Keyframe 2
* The function is just a shell, you can modify the function
* to put what you want in the function
*/
CWalkConf* CWalker::genInterpolation1(CWalkConf keyframe1,CWalkConf keyframe2 )
{
// put the interpolating function you want here :
return genInterpolationA(keyframe1, keyframe2);
// return genInterpolationAA(keyframe1, keyframe2);
}
/**
* Interpolate Keyframe 2 and Keyframe 3
* The function is just a shell, you can modify the function
* to put what you want in the function
*/
CWalkConf* CWalker::genInterpolation2(CWalkConf keyframe2,CWalkConf keyframe3 )
{
// put the interpolating function you want here :
return genInterpolationB(keyframe2, keyframe3);
}
/**
* Interpolate Keyframe 3 and Keyframe 4
* The function is just a shell, you can modify the function
* to put what you want in the function
*/
CWalkConf* CWalker::genInterpolation3(CWalkConf keyframe3,CWalkConf keyframe4 )
{
// put the interpolating function you want here :
return genInterpolationC(keyframe3, keyframe4);
}
/**
* Generate Standing Configuration in in
*/
CWalkConf CWalker::genStandConf(CPosition in)
{
CWalkConf cf;
if(in.IsEmpty())
return cf;
CPosition pos(in.x, in.y,in.z+len_thigh+len_calf,in.theta);
cf.SetPara(pos,1.5*PI, 0, 0.5*PI,1.5*PI, 0, 0.5*PI);
return cf;
}
/**
*
*/
CWalkConf CWalker::AKeyframe(CPosition from, CPosition to)
{
CWalkConf cf;
// check from is not null
if(from.IsEmpty())
return cf;
// leg length
double lenLeg = len_thigh+len_calf;
double distance = 0;
if(!to.IsEmpty()) distance = from.distance(to);
// set the minimal bound value of theta
if(distance < len_foot) distance = len_foot;
// use leg length and distance to calculate theta
double theta = calIncluded(lenLeg, lenLeg, distance)/2;
// set the maximal bound value of theta
if(theta > ulimit_ankle - 0.5*PI)
theta = ulimit_ankle - 0.5*PI;
// *** No Collision Free *** //
cf.SetPara(genStandConf(from).loca_sacroiliac,1.5*PI+theta,2*(PI-theta),0.5*PI,1.5*PI,0, 0.5*PI);
return cf;
}
/**
*
*/
CWalkConf CWalker::BKeyframe(CPosition from, CPosition to)
{
CWalkConf cf;
// check input
if(from.IsEmpty()|| to.IsEmpty())
return cf;
double d = sqrt((from.x-to.x)*(from.x-to.x) +(from.y-to.y)*(from.y-to.y) )/2;
double l = len_thigh + len_calf;
double h = sqrt(l*l-d*d);
double rota_f_hip;
double rota_f_knee;
double rota_f_ankle;
double rota_b_hip;
double rota_b_knee;
double rota_b_ankle;
// find sacroiliac
CPosition sacroiliac((from.x+to.x)/2,(from.y+to.y)/2,from.z+h,atan2(to.y-from.y, to.x-from.x));
// use sacroiliac and ankle(to) to find knee(front leg)
CIK ik;
CPosition knee = ik.IK_3D(sacroiliac, len_thigh,to, len_calf,to.x-from.x, to.y-from.y, 0);
// that knee is null means walk down, and
// the model of walk plain and walk up is
// different to walk down
if(knee.IsEmpty())
{
// find sacroiliac
CPosition sacroiliac((from.x+to.x)/2,(from.y+to.y)/2,to.z+h,atan2(to.y-from.y, to.x-from.x));
// use sacroiliac and ankle(to) to find knee(back leg)
knee = ik.IK_3D(sacroiliac, len_thigh, from, len_calf,to.x-from.x, to.y-from.y, 0);
// if knee is still null, it means can generate
// the Keyframe, then return null
if(knee.IsEmpty())
return cf;
rota_f_hip = 1.5*PI + calIncluded(l, h, d);
rota_f_knee = 0;
rota_f_ankle = 0.5*PI;
rota_b_hip = rota_f_hip -calIncluded(l, sacroiliac.distance(from), to.distance(from)) +
calIncluded(len_thigh, sacroiliac.distance(from), len_calf);
rota_b_knee = PI + calIncluded(len_thigh, len_calf,sacroiliac.distance(from));
// rota_b_ankle = 0.5*Math.PI; // No Collision Free
rota_b_ankle = 4*PI - rota_b_hip - rota_b_knee; // Collision Free
} else {
rota_b_hip = 1.5*PI - calIncluded(l, h, d);
rota_b_knee = 0;
// rota_b_ankle = 0.5*Math.PI; // No Collision Free
rota_b_ankle = 0.5*PI + calIncluded(l, h, d); // Collision Free
rota_f_hip = rota_b_hip + calIncluded(l, len_thigh, from.distance(knee));
rota_f_knee = PI + calIncluded(len_thigh, len_calf, sacroiliac.distance(to));
// rota_f_ankle = 0.5*Math.PI; // No Collision Free
rota_f_ankle = 4*PI - rota_f_hip - rota_f_knee; // Collision Free
if(rota_f_ankle < 0.5*PI) rota_f_ankle = 0.5*PI;
}
cf.SetPara(sacroiliac,rota_f_hip, rota_f_knee, rota_f_ankle,rota_b_hip, rota_b_knee, rota_b_ankle );
return cf;
}
/**
*
*/
CWalkConf CWalker::CKeyframe(CPosition from, CPosition to)
{
CWalkConf cf;
cf.SetPara(genStandConf(to).loca_sacroiliac,1.5*PI,0, 0.5*PI,1.5*PI, 1.8*PI, 0.5*PI);
return cf;
}
/**
*
*/
CWalkConf* CWalker::genInterpolationA(CWalkConf keyframe1,CWalkConf keyframe2 )
{
// check input
if(keyframe1.IsEmpty()||keyframe2.IsEmpty())
{
CWalkConf * wa=new CWalkConf;
return wa;
}
// interval for joint
double interval = 0.05;
// ################
double start = ::DoubleModule((4*PI - keyframe1.rota_b_hip- keyframe1.rota_b_knee),(2*PI));
double end = ::DoubleModule((4*PI - keyframe2.rota_b_hip- keyframe2.rota_b_knee),(2*PI));
int numKeyframe = (int)(floor((abs((start - end)/interval))));
if(numKeyframe == 0) numKeyframe++;
CWalkConf* wa1 = new CWalkConf[numKeyframe-1];
CWalkConf* watmp= new CWalkConf;
CPosition from = loca_b_ankle(keyframe2);
CPosition to = loca_f_ankle(keyframe2);
double theta = atan2(to.y-from.y, to.x-from.x);
for(int i = 1; i < numKeyframe; i++) {
double rota_f_hip = interJoint(keyframe1.rota_f_hip,keyframe2.rota_f_hip,i, numKeyframe);
double rota_f_knee = interJoint(keyframe1.rota_f_knee,keyframe2.rota_f_knee,i, numKeyframe);
double rota_f_ankle = interJoint(keyframe1.rota_f_ankle,keyframe2.rota_f_ankle,i, numKeyframe);
if(keyframe1.rota_f_ankle ==keyframe2.rota_f_ankle )
rota_f_ankle = keyframe2.rota_f_ankle;
double theta1 = interJoint(start, end, i, numKeyframe);
double rota_b_knee = interJoint(keyframe1.rota_b_knee,keyframe2.rota_b_knee,i, numKeyframe);
double rota_b_hip = ::DoubleModule((4*PI - rota_b_knee - theta1),(2*PI));
double rota_b_ankle = 4*PI - rota_b_hip - rota_b_knee;
double theta2 = rota_b_hip - PI;
CPosition loca_sacroiliac(from.x+len_calf*cos(PI-theta1)*cos(theta)
+len_thigh*cos(theta2)*cos(theta),
from.y+len_calf*cos(PI-theta1)*sin(theta)
+len_thigh*cos(theta2)*sin(theta),
from.z+len_calf*sin(PI-theta1)+len_thigh*sin(theta2),
theta);
watmp->SetPara(loca_sacroiliac,rota_f_hip,rota_f_knee,rota_f_ankle,rota_b_hip,rota_b_knee,rota_b_ankle);
*(wa1+i-1) = *watmp;
}
return wa1;
}
/**
*
*/
CWalkConf* CWalker::genInterpolationAA(CWalkConf keyframe1,CWalkConf keyframe2 )
{
// check input
if(keyframe1.IsEmpty()||keyframe2.IsEmpty())
{
CWalkConf * wa=new CWalkConf;
return wa;
}
// interval for joint
double interval = 0.05;
// ################
double start = ::DoubleModule((4*PI - keyframe1.rota_b_hip- keyframe1.rota_b_knee),(2*PI));
double end = ::DoubleModule((4*PI - keyframe2.rota_b_hip- keyframe2.rota_b_knee),(2*PI));
int numKeyframe = (int)(floor((abs((start - end)/interval))));
if(numKeyframe == 0)
numKeyframe++;
CWalkConf* wa1 = new CWalkConf[numKeyframe-1];
CWalkConf * watTmp=new CWalkConf;
CPosition from = loca_b_ankle(keyframe2);
CPosition to = loca_f_ankle(keyframe2);
CPosition f_from = loca_f_ankle(keyframe1);
CIK ik;
double theta = atan2(to.y-from.y, to.x-from.x);
for(int i = 1; i < numKeyframe; i++)
{
double theta1= interJoint(start, end, i, numKeyframe);
double rota_b_knee = interJoint(keyframe1.rota_b_knee,keyframe2.rota_b_knee,i, numKeyframe);
double rota_b_hip = ::DoubleModule((4*PI - rota_b_knee - theta1),(2*PI));
double rota_b_ankle = 4*PI - rota_b_hip - rota_b_knee;
double theta2 = rota_b_hip - PI;
CPosition loca_sacroiliac(from.x+len_calf*cos(PI-theta1)*cos(theta)
+len_thigh*cos(theta2)*cos(theta),
from.y+len_calf*cos(PI-theta1)*sin(theta)
+len_thigh*cos(theta2)*sin(theta),
from.z+len_calf*sin(PI-theta1)+len_thigh*sin(theta2),
theta);
CPosition f_ankle = interCartesian(f_from, to, i, numKeyframe);
CPosition f_knee = ik.IK_3D(loca_sacroiliac, len_thigh,
f_ankle, len_calf,
to.x-from.x, to.y-from.y, 0);
double l = loca_sacroiliac.distance(f_ankle);
double k = loca_sacroiliac.distance(from);
double m = from.distance(f_knee);
double rota_f_hip = PI*2 - rota_b_ankle +calIncluded(len_thigh, k, m);
double rota_f_knee =::DoubleModule((PI +calIncluded(len_thigh, len_calf, l)),(PI*2));
double rota_f_ankle = interJoint(keyframe1.rota_f_ankle,keyframe2.rota_f_ankle,i, numKeyframe );
if(keyframe1.rota_f_ankle ==keyframe2.rota_f_ankle )
rota_f_ankle = keyframe2.rota_f_ankle;
watTmp->SetPara(loca_sacroiliac,
rota_f_hip,
rota_f_knee,
rota_f_ankle,
rota_b_hip,
rota_b_knee,
rota_b_ankle);
*(wa1+i-1) = *watTmp;
}
return wa1;
}
/**
*
*/
CWalkConf* CWalker::genInterpolationB(CWalkConf keyframe2,CWalkConf keyframe3 )
{
if(keyframe2.IsEmpty()||keyframe3.IsEmpty())
{
CWalkConf * wa=new CWalkConf;
return wa;
}
double interval = 0.05; // interval for joint
int numKeyframe = (int)(floor((abs((keyframe2.rota_f_hip -keyframe3.rota_f_hip)/interval))));
if(numKeyframe == 0) numKeyframe++;
CWalkConf* wa2 = new CWalkConf[numKeyframe-1];
CWalkConf* watTmp=new CWalkConf;
CPosition from = loca_b_ankle(keyframe2);
CPosition to = loca_f_ankle(keyframe2);
double theta = atan2(to.y-from.y, to.x-from.x);
for(int i = 1; i < numKeyframe; i++)
{
double rota_f_hip = interJoint(keyframe2.rota_f_hip,keyframe3.rota_f_hip,i, numKeyframe);
double rota_f_knee = interJoint(keyframe2.rota_f_knee,keyframe3.rota_f_knee,i, numKeyframe);
double rota_f_ankle = 4*PI - rota_f_hip - rota_f_knee;
if(rota_f_ankle < keyframe3.rota_f_ankle)
rota_f_ankle = keyframe3.rota_f_ankle;
double theta1 = ::DoubleModule((rota_f_hip + rota_f_knee - 3*PI),(int)(2*PI));
double theta2 = rota_f_hip - PI;
CPosition loca_sacroiliac(to.x+len_calf*cos(theta1)*cos(theta)
+len_thigh*cos(theta2)*cos(theta),
to.y+len_calf*cos(theta1)*sin(theta)
+len_thigh*cos(theta2)*sin(theta),
to.z+len_calf*sin(theta1)+len_thigh*sin(theta2),
theta);
watTmp->SetPara(loca_sacroiliac,
rota_f_hip,
rota_f_knee,
rota_f_ankle,
interJoint(keyframe2.rota_b_hip,keyframe3.rota_b_hip,i, numKeyframe),
interJoint(keyframe2.rota_b_knee,keyframe3.rota_b_knee,i, numKeyframe),
keyframe2.rota_b_ankle);
*(wa2+i-1) =*watTmp;
}
return wa2;
}
/**
*
*/
CWalkConf * CWalker::genInterpolationC(CWalkConf keyframe3,CWalkConf keyframe4 )
{
if(keyframe3.IsEmpty()||keyframe4.IsEmpty())
{
CWalkConf * wa=new CWalkConf;
return wa;
}
double interval = 0.001; // interval for joint
int numKeyframe = (int)(floor((abs((keyframe3.rota_b_hip -keyframe4.rota_b_hip)/interval))));
if(numKeyframe == 0) numKeyframe++;
CWalkConf* wa3 = new CWalkConf[numKeyframe-1];
CWalkConf* watTmp=new CWalkConf;
for(int i = 1; i < numKeyframe; i++)
{
watTmp->SetPara(keyframe3.loca_sacroiliac,
keyframe3.rota_f_hip,
keyframe3.rota_f_knee,
keyframe3.rota_f_ankle,
interJoint(keyframe3.rota_b_hip,
keyframe4.rota_b_hip,i, numKeyframe),
interJoint(keyframe3.rota_b_knee,
keyframe4.rota_b_knee,i, numKeyframe),
keyframe3.rota_b_ankle);
*(wa3+i-1) = *watTmp;
}
return wa3;
}
/**
* find position of knee of the front leg
*/
CPosition CWalker::loca_f_knee(CWalkConf w)
{
CPosition p=findPosition(w.loca_sacroiliac, len_thigh, w.rota_f_hip);
return p;
}
/**
* find position of ankle of the front leg
*/
CPosition CWalker::loca_f_ankle(CWalkConf w)
{
CPosition p=findPosition(loca_f_knee(w), len_calf,w.rota_f_hip + w.rota_f_knee);
return p;
}
/**
* find position of toetip of the front leg
*/
CPosition CWalker::loca_f_toetip(CWalkConf w)
{
CPosition p=findPosition(loca_f_ankle(w), len_foot,w.rota_f_hip + w.rota_f_knee + w.rota_f_ankle);
return p;
}
/**
* find position of knee of the back leg
*/
CPosition CWalker::loca_b_knee(CWalkConf w)
{
CPosition p=findPosition(w.loca_sacroiliac, len_thigh, w.rota_b_hip);
return p;
}
/**
* find position of ankle of the back leg
*/
CPosition CWalker::loca_b_ankle(CWalkConf w)
{
CPosition p=findPosition(loca_b_knee(w), len_calf,w.rota_b_hip + w.rota_b_knee);
return p;
}
/**
* find position of toetip of the back leg
*/
CPosition CWalker::loca_b_toetip(CWalkConf w)
{
CPosition p=findPosition(loca_b_ankle(w), len_foot,w.rota_b_hip + w.rota_b_knee + w.rota_b_ankle);
return p;
}
/**
* find position
*/
CPosition CWalker::findPosition(CPosition l, double len, double theta)
{
CPosition p(l.x+len*cos(theta)*cos(l.theta),l.y+len*cos(theta)*sin(l.theta),l.z+len*sin(theta),l.theta);
return p;
}
/**
* give you first edge a, second edge b and third edge c
* find the included of a and b
*/
double CWalker::calIncluded(double a, double b, double c)
{
return acos((a*a+b*b-c*c)/(2*a*b));
}
/**
* give you first egde, second edge and the included of a and b
* find third edge x
*/
double CWalker::antiCalIncluded(double a, double b, double theta)
{
return sqrt(a*a + b*b - 2*a*b*cos(theta));
}
/**
* interpolation for joint space
*/
double CWalker::interJoint(double s, double e, int i, int total)
{
if(e - s > PI)
s += 2*PI;
else if(s - e > PI)
e += 2*PI;
return (s*(total-i) + e*i)/total;
}
/**
* interpolation for cartesian space
*/
CPosition CWalker::interCartesian(CPosition s, CPosition e, int i, int total)
{
CPosition p ;
p.x = (s.x*(total-i) + e.x*i)/total;
p.y = (s.y*(total-i) + e.y*i)/total;
p.z = (s.z*(total-i) + e.z*i)/total;
return p;
}