body.cpp

robocuo相关资料robocuo相关资料

#include "body.h"
#include "world.h"

body::body(world*wm)
{
}

void body::analyze()
{
	l[Neck].mat=l[Torso].mat*
		Matrix::translate(0+0,0+0,0.09+0)*
		Matrix::rotateZ(j[HJ1].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[Head].mat=l[Neck].mat*
		Matrix::translate(0+0,0+0,0.065+-0.005)*
		Matrix::rotateX(j[HJ2].angle)*
		Matrix::translate(-(0),-(0),-(-0.005));

	l[RightShoulder].mat=l[Torso].mat*
		Matrix::translate(0.098+0,0+0,0.075+0)*
		Matrix::rotateX(j[RAJ1].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[LeftShoulder].mat=l[Torso].mat*
		Matrix::translate(-0.098+0,0+0,0.075+0)*
		Matrix::rotateX(j[LAJ1].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[RightUpperarm].mat=l[RightShoulder].mat*
		Matrix::translate(0.01+-0.01,0.02+-0.02,0+0)*
		Matrix::rotateZ(j[RAJ2].angle)*
		Matrix::translate(-(-0.01),-(-0.02),-(0));

	l[LeftUpperarm].mat=l[LeftShoulder].mat*
		Matrix::translate(-0.01+0.01,0.02+-0.02,0+0)*
		Matrix::rotateZ(j[RAJ2].angle)*
		Matrix::translate(-(0.01),-(-0.02),-(0));

	l[RightElbow].mat=l[RightUpperarm].mat*
		Matrix::translate(-0.01+0,0.07+0,0.009+0)*
		Matrix::rotateY(j[RAJ3].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[LeftElbow].mat=l[LeftUpperarm].mat*
		Matrix::translate(0.01+0,0.07+0,0.009+0)*
		Matrix::rotateY(j[LAJ3].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[RightLowerArm].mat=l[RightElbow].mat*
		Matrix::translate(0+0,0.05+-0.05,0+0)*
		Matrix::rotateZ(j[RAJ4].angle)*
		Matrix::translate(-(0),-(-0.05),-(0));

	l[LeftLowerArm].mat=l[LeftElbow].mat*
		Matrix::translate(0+0,0.05+-0.05,0+0)*
		Matrix::rotateZ(j[LAJ4].angle)*
		Matrix::translate(-(0),-(-0.05),-(0));

	l[RightHip1].mat=l[Torso].mat*
		Matrix::translate(0.055+0,-0.01+0,-0.115+0)*
		Matrix::rotate_arbitrary_axis(Vector3d(-1,0,1),j[RLJ1].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[LeftHip1].mat=l[Torso].mat*
		Matrix::translate(-0.055+0,-0.01+0,-0.115+0)*
		Matrix::rotate_arbitrary_axis(Vector3d(-1,0,-1),j[LLJ1].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[RightHip2].mat=l[RightHip1].mat*
		Matrix::translate(0+0,0+0,0+0)*
		Matrix::rotateY(j[RLJ2].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[LeftHip2].mat=l[LeftHip1].mat*
		Matrix::translate(0+0,0+0,0+0)*
		Matrix::rotateY(j[LLJ2].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[RightThigh].mat=l[RightHip2].mat*
		Matrix::translate(0+0,0.01+-0.01,-0.04+0.04)*
		Matrix::rotateX(j[RLJ3].angle)*
		Matrix::translate(-(0),-(-0.01),-(0.04));

	l[LeftThigh].mat=l[LeftHip2].mat*
		Matrix::translate(0+0,0.01+-0.01,-0.04+0.04)*
		Matrix::rotateX(j[LLJ3].angle)*
		Matrix::translate(-(0),-(-0.01),-(0.04));

	l[RightShank].mat=l[RightThigh].mat*
		Matrix::translate(0+0,0.005+-0.01,-0.125+0.045)*
		Matrix::rotateX(j[RLJ4].angle)*
		Matrix::translate(-(0),-(-0.01),-(0.045));

	l[LeftShank].mat=l[LeftThigh].mat*
		Matrix::translate(0+0,0.005+-0.01,-0.125+0.045)*
		Matrix::rotateX(j[LLJ4].angle)*
		Matrix::translate(-(0),-(-0.01),-(0.045));

	l[RightAnkle].mat=l[RightShank].mat*
		Matrix::translate(0+0,-0.01+0,-0.055+0)*
		Matrix::rotateX(j[RLJ5].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[LeftAnkle].mat=l[LeftShank].mat*
		Matrix::translate(0+0,-0.01+0,-0.055+0)*
		Matrix::rotateX(j[LLJ5].angle)*
		Matrix::translate(-(0),-(0),-(0));

	l[RightFoot].mat=l[RightAnkle].mat*
		Matrix::translate(0+0,0.03+-0.03,-0.035+0.035)*
		Matrix::rotateY(j[RLJ6].angle)*
		Matrix::translate(-(0),-(-0.03),-(0.035));

	l[LeftFoot].mat=l[LeftAnkle].mat*
		Matrix::translate(0+0,0.03+-0.03,-0.035+0.035)*
		Matrix::rotateY(j[LLJ6].angle)*
		Matrix::translate(-(0),-(-0.03),-(0.035));

	for(int i=0;i<23;i++)
	{
		Vector3d v=l[i].mat.getVector3d();
		//cout<<limb_name[i]<<"\t"<<v<<endl;
	}
}