📄 mysdopenglview.cpp
字号:
&model->colorList[j].rs,model->colorList[j].spec,0);
// setColor(&model->colorList[j].rd,&model->colorList[j].gd,
// &model->colorList[j].bd,model->colorList[j].spec,0);
j++;
nextIndex=model->colorList[j].index;
}
if(drawSetting<=LINE_POLYGON)
glBegin(GL_POLYGON);
else
glBegin(GL_LINE_LOOP);
glNormal3fv(&model->vertexList[i].nx);
glVertex3fv(&model->vertexList[i++].x);
while(model->vertexList[i].draw)
{
glNormal3fv(&model->vertexList[i].nx);
glVertex3fv(&model->vertexList[i++].x);
}
glEnd();
}while(i<model->vertexCount);
}
else do
{
if(i==nextIndex)
{
setColor(&model->colorList[j].ra,&model->colorList[j].rd,
&model->colorList[j].rs,model->colorList[j].spec,0);
j++;
nextIndex=model->colorList[j].index;
}
if(drawSetting<=LINE_POLYGON)
glBegin(LINE_POLYGON);
else
glBegin(GL_LINE_LOOP);
glVertex3fv(&model->vertexList[i++].x);
while(model->vertexList[i].draw)
{
glVertex3fv(&model->vertexList[i++].x);
}
glEnd();
}while(i<model->vertexCount);
}
else
{
currColor=-1;
if(drawSetting==LINE_INDEPENDENT)
{
glBegin(GL_LINES);
do
{
if(model->lineList[i].colorIndex!= currColor)
{
currColor=model->lineList[i].colorIndex;
setColor(&model->colorList[currColor].ra,
&model->colorList[currColor].rd,
&model->colorList[currColor].rs,
model->colorList[currColor].spec,0);
}
glVertex3fv(&model->lineList[i++].x);
glVertex3fv(&model->lineList[i++].x);
}while(i<model->lineCount);
glEnd();
}
if(drawSetting==LINE_STRIPS)
do
{
glBegin(GL_LINE_STRIP);
if(model->lineStripList[i].colorIndex!= currColor)
{
currColor=model->lineStripList[i].colorIndex;
setColor(&model->colorList[currColor].ra,
&model->colorList[currColor].rd,
&model->colorList[currColor].rs,
model->colorList[currColor].spec,0);
}
glVertex3fv(&model->lineStripList[i++].x);
while(model->lineStripList[i].draw)
{
glVertex3fv(&model->lineStripList[i++].x);
}
glEnd();
}while(i<model->lineStripCount);
}
}
}
void CMySDOpenGLView::setColor(const GLfloat *ambient,const GLfloat *diffuse,const GLfloat *specular,GLfloat shininess,GLboolean stereo)
{
GLfloat c;
GLfloat ca[4];
if(!stereo){
// glMaterialfv(GL_FRONT,GL_AMBIENT,ambient);
glMaterialfv(GL_FRONT,GL_DIFFUSE,diffuse);
// glMaterialfv(GL_FRONT,GL_SPECULAR,specular);
// glMaterialf(GL_FRONT,GL_SHININESS,shininess);
glColor4fv(diffuse);
}
else{
c=0.27*ambient[0]+0.59*ambient[1]+0.14*ambient[2];
ca[0]=ca[1]=ca[2]=c;
glMaterialfv(GL_FRONT,GL_AMBIENT,ca);
ca[0]=ca[1]=ca[2]=c;
ca[3]=diffuse[3];
c=0.27*diffuse[0]+0.59*diffuse[1]+0.14*diffuse[2];
glMaterialfv(GL_FRONT,GL_DIFFUSE,ca);
glColor4f(c,c,c,diffuse[3]);
ca[0]=ca[1]=ca[2]=c;
c=0.27*specular[0]+0.59*specular[1]+0.14*specular[2];
glMaterialfv(GL_FRONT,GL_SPECULAR,ca);
glMaterialf(GL_FRONT,GL_SHININESS,shininess);
}
}
void CMySDOpenGLView::Animation(double Angle1, double Angle2)
{
glPushMatrix();
glRotatef(Angle1,0.0,1.0,0.0);
DawObj(model2);
glTranslatef(8.0,0.0,0.0);
glRotatef(Angle2,1.0,0.0,0.0);
DawObj(model3);
glPopMatrix();
}
void CMySDOpenGLView::DrawBase(void)
{
DawCylinder(cylinder_obj,0.0,-2.0,0.0,270,1.0, 0.0,0.0,0.5,0.5,4);
/* glPushMatrix();
glTranslatef(0.0,-2.0,0.0);
glRotatef(270,1.0,0.0,0.0);
gluQuadricOrientation(cylinder_obj,GLU_INSIDE);
gluDisk(cylinder_obj,0,0.5,24,4);
glPushMatrix();
glTranslatef(0.0,0.0,4.0);
gluQuadricOrientation(cylinder_obj,GLU_OUTSIDE);
gluDisk(cylinder_obj,0,0.5,24,4);
glPopMatrix();
gluCylinder(cylinder_obj,0.5,0.5,4,20,2);
glPopMatrix();
/*
/*
glTranslatef(0.0,0.0,-0.5);
glRotatef(20,0.0,1.0,0.0);
glTranslatef(0.0,0.0,-0.5);
*/
DawCylinder(cylinder_obj,0.0,1.0,0.0,90,0.0, 1.0,0.0,0.4,0.4,2);
/* glPushMatrix();
glRotatef(270,0.0,1.0,0.0);
glTranslatef(0.0,1.0,0.0);
gluQuadricOrientation(cylinder_obj,GLU_INSIDE);
gluDisk(cylinder_obj,0,0.4,24,1);
glPushMatrix();
glTranslatef(0.0,0.0,2.0);
gluQuadricOrientation(cylinder_obj,GLU_OUTSIDE);
gluDisk(cylinder_obj,0,0.4,24,1);
glPopMatrix();
gluCylinder(cylinder_obj,0.4,0.4,2,20,2);
glPopMatrix();
*/
}
void CMySDOpenGLView::Animation1(double Angle1, double Angle2, double Angle3,double Angle4)
{
// DrawPoint(0.0,1.0,0.0,0.2);
// DrawPoint(0.0,1.0,2.0,0.2);
// DrawPoint(2.3,1.0,0.0,0.2);
// DrawPoint(2.3,1.0,6.0,0.2);
// DrawPoint(2.3,-5,0,0.2);
glEnable(GL_COLOR_MATERIAL);
glColor4f(0.0f,0.0f,1.0f,0.0f);
glDisable(GL_COLOR_MATERIAL);
glPushMatrix();
glRotatef(Angle1,0.0,1.0,0.0);
DrawBase();
glTranslatef(1.5,1.0,0.0);
glRotatef(Angle2,1.0,0.0,0.0);
DrawArm(0.8);
DawCylinder(cylinder_obj,1.3,0.0,2.0,270,0.0, 1.0,0.0,0.3,0.3,1.8);
glTranslatef(0.8,0.0,2.0);
glRotatef(Angle3,1.0,0.0,0.0);
DrawArm(0.7);
glRotatef(Angle4,0.0,0.0,1.0);
DawCylinder(cylinder_obj,0,0.0,2.0,0.0,0.0, 0.0,0.0,0.3,0.3,1.8);
glTranslatef(0.0,0.0,4.0);
glColorMaterial(GL_FRONT,GL_DIFFUSE);
glEnable(GL_COLOR_MATERIAL);
DrawAxisArrow();
glDisable(GL_COLOR_MATERIAL);
glPopMatrix();
}
void CMySDOpenGLView::DawCylinder(GLUquadric * cylinder_obj,double x,double y,double z,double angle,double vx, double vy,double vz,double base,double top,double height)
{
glPushMatrix();
glTranslatef(x,y,z);
glRotatef(angle,vx,vy,vz);
gluQuadricOrientation(cylinder_obj,GLU_INSIDE);
gluDisk(cylinder_obj,0,top,24,4);
glPushMatrix();
glTranslatef(0.0,0.0,height);
gluQuadricOrientation(cylinder_obj,GLU_OUTSIDE);
gluDisk(cylinder_obj,0,base,24,4);
glPopMatrix();
gluCylinder(cylinder_obj,base,top,height,20,2);
glPopMatrix();
}
void CMySDOpenGLView::DrawArm(GLfloat bl)
{//GLfloat bl=0.8;
GLfloat p1[]={-0.5*bl,1.0*bl,-0.5*bl},p2[]={-0.5*bl,-1.0*bl,-0.5*bl},
p3[]={-0.5*bl,-0.5*bl,3.5*bl},p4[]={-0.5*bl,0.5*bl,3.5*bl},
p5[]={0.5*bl,1.0*bl,-0.5*bl},p6[]={0.5*bl,-1.0*bl,-0.5*bl},
p7[]={0.5*bl,-0.5*bl,3.5*bl},p8[]={0.5*bl,0.5*bl,3.5*bl};
GLfloat m1[]={-1.0,0.0,0.0}, m2[]={1.0,0.0,0.0},
m3[]={0.0,0.0,-1.0}, m4[]={0.0,0.0,1.0},
m5[]={0.0,8.0/sqrt(65.0),1.0/sqrt(65.0)},
m6[]={0.0,-8.0/sqrt(65.0),-1.0/sqrt(65.0)};
glBegin(GL_QUADS);
glNormal3fv(m1);
glVertex3fv(p1);
glVertex3fv(p2);
glVertex3fv(p3);
glVertex3fv(p4);
glNormal3fv(m2);
glVertex3fv(p5);
glVertex3fv(p6);
glVertex3fv(p7);
glVertex3fv(p8);
glNormal3fv(m3);
glVertex3fv(p1);
glVertex3fv(p2);
glVertex3fv(p6);
glVertex3fv(p5);
glNormal3fv(m4);
glVertex3fv(p3);
glVertex3fv(p4);
glVertex3fv(p8);
glVertex3fv(p7);
glNormal3fv(m5);
glVertex3fv(p1);
glVertex3fv(p5);
glVertex3fv(p8);
glVertex3fv(p4);
glNormal3fv(m6);
glVertex3fv(p2);
glVertex3fv(p6);
glVertex3fv(p7);
glVertex3fv(p3);
glEnd();
}
void CMySDOpenGLView::DrawPoint(GLfloat x,GLfloat y,GLfloat z,GLfloat size)
{
glPushMatrix();
glTranslatef(x,y,z);
glColorMaterial(GL_FRONT,GL_DIFFUSE);
glEnable(GL_COLOR_MATERIAL);
glColor3f(1.f,0.f,0.f);
gluSphere(cylinder_obj,size,20,20);
glDisable(GL_COLOR_MATERIAL);
glPopMatrix();
}
//构造各关节指数变换矩阵exp(ξθ),并计算gst
void CMySDOpenGLView::MakeSE3(void)
{
//各关节长度
//#define PI 3.1415926;
l0=1;
l1=2;
l2=2.3;
l3=4;
//各关节旋转长度
double cAngle1=inputAngle1*3.1415926/180;
double cAngle2=inputAngle2*3.1415926/180;
double cAngle3=inputAngle3*3.1415926/180;
double cAngle4=inputAngle4*3.1415926/180;
//各关节指数变换矩阵exp(ξθ)，及gst0
// double v1[]={cos(cAngle1),0,sin(cAngle1),0,0,1,0,0,-sin(cAngle1),0,cos(cAngle1),0,0,0,0,1};
// double v2[]={1,0,0,0,0,cos(cAngle2),-sin(cAngle2),l0*(1-cos(cAngle2)),0,sin(cAngle2),cos(cAngle2),-l0*sin(cAngle2),0,0,0,1};
// double v3[]={1,0,0,0,0,cos(cAngle3),-sin(cAngle3),l0*(1-cos(cAngle3))+l1*sin(cAngle3),0,sin(cAngle3),cos(cAngle3),-l0*sin(cAngle3)+l1*(1-cos(cAngle3)),0,0,0,1};
// double v4[]={cos(cAngle4),-sin(cAngle4),0,l2*(1-cos(cAngle4))+l0*sin(cAngle4),sin(cAngle4),cos(cAngle4),0,-l2*sin(cAngle4)+l0*(1-cos(cAngle4)),0,0,1,0,0,0,0,1};
double vst0[]={1,0,0,l2,0,1,0,l0,0,0,1,l1+l3,0,0,0,1};
// CMatrix g1,g2,g3,g4,gst0,
CMatrix g1=Makeg1(cAngle1);
CMatrix g2=Makeg2(cAngle2);
CMatrix g3=Makeg3(cAngle3);
CMatrix g4=Makeg4(cAngle4);
// CMatrix g1(4,v1);
// CMatrix g2(4,v2);
// CMatrix g3(4,v3);
// CMatrix g4(4,v4);
CMatrix gst0(4,vst0);
ig1=g1;ig2=g2;ig3=g3;ig4=g4;igst0=gst0;
q1.x=0;q1.y=l0;q1.x=0;
q2.x=0;q2.y=l0;q2.z=l1;
q3.x=l2;q3.y=l0;q3.z=0;
omiga1.x=0; omiga1.y=1;omiga1.z=0;
omiga2.x=1; omiga2.y=0;omiga2.z=0;
omiga3=omiga2;
omiga4.x=0; omiga4.y=0;omiga4.z=1;
//计算末端执行器相对于基础坐标系的位形矩阵gst
g=g1*g2*g3*g4*gst0;
// g=g1;
// CString m_strMatrix3 = g.ToString(" ");
// AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
}
//计算机器人运动学正解，并求出如何在OPENGL中画出
void CMySDOpenGLView::CoumpterFKinematics(CMatrix g)
{
double pVectorx[4]={0};
double pVectory[4]={0};
double pVectorz[4]={0};
double pVectorp[4]={0};
//取出gst中各向量，分别对应x,y,z 轴要到达的位置，以及末端执行器原点到达的位置
g.GetColVector(0, pVectorx);
g.GetColVector(1, pVectory);
g.GetColVector(2, pVectorz);
g.GetColVector(3, pVectorp);
// m={pVectorx[0],pVectorx[1],pVectorx[2],pVectorx[0]pVectory,pVectorp};
//g.Transpose();
//x轴到达的位置
vectorx.x=pVectorx[0];
vectorx.y=pVectorx[1];
vectorx.z=pVectorx[2];
//y轴到达的位置
vectory.x=pVectory[0];
vectory.y=pVectory[1];
vectory.z=pVectory[2];
//原点到达的位置
vectorp.x=pVectorp[0];
vectorp.y=pVectorp[1];
vectorp.z=pVectorp[2];
//原来x,y轴的位置
CVector3 vx(1.0,0.0,0.0);
CVector3 vy(0.0,1.0,0.0);
CVector3 v;
//double x=abs(1e-8-1);
//BOOL c=abs(vectorx.x-vx.x)<(1e-4);
//if((fabs(vectorx.x-vx.x)<1e-4)&&(fabs(vectorx.y-vx.y)<1e-4)&&(fabs(vectorx.z-vx.z)<1e-4))
//{vector=vectory;v=vy;}
//else {vector=vectorx;v=vx;}
//计算x轴绕某轴旋转的角度 ω1点乘ω2=||ω1||||ω2||cosθ，及转轴 ω1叉乘ω2=ω||ω1||||ω2||sinθ
double tempx=vectorx.Multiply(vectorx,vx);
A1=acos(tempx)*180/3.1415926;
vectorx=vectorx.Cross(vx,vectorx);
//vectorx=vectorx.Normalize(vectorx);
//求绕某轴旋转后y轴到达的位置，运用公式exp(ωθ)*y
double pVector2[3]={vy.x,vy.y,vy.z};
CMatrix g2(3,1,pVector2);
//求sinθ
double Magnitude;
Magnitude = Mag(vectorx);
// 将转轴单位化
vectorx=vectorx.Normalize(vectorx);
//计算exp(ωθ)=I+ω^sinθ+ω^2(1-cosθ)=I*cosθ+ω^sinθ+ωωT(1-cosθ)
CMatrix MI;
MI.MakeUnitMatrix(3);
//计算ω^
double pO[9]={0,-vectorx.z,vectorx.y,vectorx.z,0,-vectorx.x,-vectorx.y,vectorx.x,0};
CMatrix MO(3,pO);
// CString m_strMatrix3 = MO.ToString(" ");
// AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
////计算ωωT
double pO2[9]={vectorx.x*vectorx.x,vectorx.x*vectorx.y,vectorx.x*vectorx.z,vectorx.y*vectorx.x,vectorx.y*vectorx.y,vectorx.y*vectorx.z,vectorx.z*vectorx.x,vectorx.z*vectorx.y,vectorx.z*vectorx.z};
CMatrix MO2(3,pO2);
//计算exp(ωθ)
MI=MI*tempx+MO*Magnitude+MO2*(1.0-tempx);
// CString m_strMatrix3 = MI.ToString(" ");
// AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
//exp(ωθ)*y, 变成新的y轴
g2=MI*g2;
g2.GetColVector(0, pVector2);
vy.x=pVector2[0];
vy.y=pVector2[1];
vy.z=pVector2[2];
//计算x轴绕某轴旋转的角度，及转轴
double tempy=vectory.Multiply(vectory,vy);
A2=acos(tempy)*180/3.1415926;
vectory=vectory.Cross(vy,vectory);
//将上述转轴变换到当前坐标系以符合OPENGL的规则，运用公式exp-1(ωθ)*ω'
double pVector1[3]={vectory.x,vectory.y,vectory.z};
CMatrix g1(3,1,pVector1);
MI.InvertGaussJordan();
g1=MI*g1;
g1.GetColVector(0, pVector1);
vectory.x=pVector1[0];
vectory.y=pVector1[1];
vectory.z=pVector1[2];
}
void CMySDOpenGLView::DrawFKinematics(void)
{
glPushMatrix();
glTranslatef(vectorp.x,vectorp.y,vectorp.z);
if((fabs(vectorx.x)!=0.0)||(fabs(vectorx.y)!=0.0)||(fabs(vectorx.z)!=0.0))
{ // glPushMatrix();
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -