📄 mysdopenglview.cpp
字号:
glRotatef(A1,vectorx.x,vectorx.y,vectorx.z);
// glPopMatrix();
}
if((fabs(vectory.x)!=0.0)||(fabs(vectory.y)!=0.0)||(fabs(vectory.z)!=0.0))
{ // glPushMatrix();glMultMatrix
//GetFloatv(TRANSPOSE_MODELVIEW_MATRIX_ARB,m);
glRotatef(A2,vectory.x,vectory.y,vectory.z);
// glPopMatrix();
}
//glBegin();
// glMultMatrixd(g.m_pData);
//glEnd();
glColorMaterial(GL_FRONT,GL_DIFFUSE);
glEnable(GL_COLOR_MATERIAL);
DrawAxisArrow();
glDisable(GL_COLOR_MATERIAL);
glPopMatrix();
}
//运动学逆解问题1
BOOL CMySDOpenGLView::InverseKinematics1(CVector3 *p,CVector3 *q,CVector3 *r,CVector3 *axle,double* Angle)
{ CVector3 u,v,u1,v1;
// u=u.Vector(*p,*r);
//u=p-r v=q-r
u=*p-*r;
v=*q-*r;
double temp;
// temp=u.Multiply(*axle,u);
temp=(*axle)*u;
CVector3 tempc;
CVector3 tempcu,tempcv;
// tempc=tempc.MultiplyVectorByScaler(*axle,temp);
tempcu=(*axle)*temp;
// u1=u1.Vector(u,tempc);
//u'=u-ωωTu,v'=u-ωωTv
u1=u-tempcu;
// temp=v.Multiply(*axle,v);
temp=(*axle)*v;
// tempc=tempc.MultiplyVectorByScaler(*axle,temp);
tempcv=(*axle)*temp;
// v1=v1.Vector(v,tempc);
v1=v-tempcv;
//有解条件||u'||=||v'|| ωTu=ωTv
//本该是 if(Mag(u1)!=Mag(v1))由于有误差，所以改掉
// double a=Mag(u1)-Mag(v1);
if((fabs(Mag(u1)-Mag(v1))>1e-5)||(fabs(Mag(tempcu)-Mag(tempcv))>1e-5)) return false;
// temp=u1.Multiply(u1,v1);
temp=u1*v1;
// tempc=tempc.Cross(u1,v1);
tempc=u1^v1;
double temp1;
// temp1=u1.Multiply(*axle, tempc);
temp1=(*axle)*tempc;
//θ=atan2(ωT(u'xv'),u'Tv')
//有无穷解
if(u1==(0,0,0)) *Angle=1e8;
else{ *Angle=atan2(temp1,temp);
*Angle=(*Angle)*180/PI;
}
return true;
}
//运动学逆解问题2
BOOL CMySDOpenGLView::InverseKinematics2(CVector3 *p,CVector3 *q,CVector3 *r,CVector3 *axle1,CVector3 *axle2,int *NOfAngle,double *Angle1,double *Angle2)
{CVector3 u,v,z,c;
//u=p-r v=q-r
u=*p-*r;
v=*q-*r;
double ar,ba,ga2,ga;
double temp12,temp1,temp2;
temp12=(*axle1)*(*axle2);
temp1=(*axle1)*v;
temp2=(*axle2)*u;
//α=((ω1Tω2)ω2Tu-ω1Tv)/((ω1Tω2)^2-1)
//β=((ω1Tω2)ω1Tv-ω2Tu)/((ω1Tω2)^2-1)
ar=(temp12*temp2-temp1)/(temp12*temp12-1.0);
ba=(temp12*temp1-temp2)/(temp12*temp12-1.0);
// if(temp1!=temp2) return false;
double tempuu,tempm12;
tempuu=Mag(u)*Mag(u);
CVector3 tempc;
tempc=(*axle1)^(*axle2);
tempm12=Mag(tempc);
//γ^2=(||u||^2-α^2-β^2-2αβω1Tω2)/||ω1xω2||^2
ga2=(tempuu-ar*ar-ba*ba-2.0*ar*ba*temp12)/(tempm12*tempm12);
// γ^2=0一解，γ^2>0两解,γ^2<0无解
//本该是 if(ga2==0) NOfAngle=1 由于sqrt()有误差，所以改掉
if(fabs(ga2)<1e-8) *NOfAngle=1;
else if(ga2>0) *NOfAngle=2;
else { return false;}
//z=αω1+βω2+γ(ω1xω2) c=z+r
ga2=fabs(ga2);
ga=sqrt(ga2);
z=(*axle1)*ar+(*axle2)*ba+tempc*ga;
c=z+*r;
//利用子问题一解exp(ξ2θ2)p=c exp(-ξ1θ1)q=c
BOOL F1,F2,F3,F4;
F1=InverseKinematics1(p,&c,r,axle2,Angle2);
CVector3 axle11;
axle11=-(*axle1);
F2=InverseKinematics1(q,&c,r,&axle11,Angle1);
if(*NOfAngle==2){
int i=1;
if((F1&F2)==false) {i=0;*NOfAngle=1;}
ga=-sqrt(ga2);
z=(*axle1)*ar+(*axle2)*ba+tempc*ga;
c=z+*r;
F3=InverseKinematics1(p,&c,r,axle2,Angle2+i);
F4=InverseKinematics1(q,&c,r,&axle11,Angle1+i);
}
if(((F1&F2)==false)&&((F3&F4)==false))
{*NOfAngle=0;return false;}
return true;
}
//运动学逆解问题3
BOOL CMySDOpenGLView::InverseKinematics3(CVector3 *p,CVector3 *q,CVector3 *r,CVector3 *axle,double *delta,int *NOfAngle,double *Angle)
{ CVector3 u,v,u1,v1;
u=*p-*r;
v=*q-*r;
//u'=u-ωωTu,v'=u-ωωTv
double temp;
temp=(*axle)*u;
CVector3 tempc;
tempc=(*axle)*temp;
u1=u-tempc;
temp=(*axle)*v;
tempc=(*axle)*temp;
v1=v-tempc;
temp=u1*v1;
tempc=u1^v1;
//θ0=atan2(ωT(u'xv'),u'Tv')
double temp1;
temp1=(*axle)*tempc;
double Angle0=atan2(temp1,temp);
//δ'^2=δ^2-||ωT(p-q)||^2
double delta12;
tempc=*p-*q;
temp=(*axle)*tempc;
delta12=(*delta)*(*delta)-temp*temp;
//δ'^2<0无解
if(fabs(delta12)<1e-8) delta12=0;
else if(delta12<0){return false;}
//arccos((||u'||^2+||v'||^2-δ'^2)/2||u'||||v'||)
double mu1,mv1;
mu1=Mag(u1);
mv1=Mag(v1);
temp=acos((mu1*mu1+mv1*mv1-delta12)/(2*mu1*mv1));
//θ=θ0+/-arccos((||u'||^2+||v'||^2-δ'^2)/2||u'||||v'||)
//q在轴线上，无穷多解
if(v1==(0,0,0)) { *NOfAngle=1e8;*Angle=1e8;return true;}
if(fabs(temp)<1e-8) {*NOfAngle=1;*Angle=Angle0*180/PI;}
else { *NOfAngle=2;
*Angle=(Angle0+temp)*180/PI;
//寻找所需转的最小角度
if(fabs(*Angle)>(fabs(*Angle-360))) *Angle-=360;
*(Angle+1)=(Angle0-temp)*180/PI;
if(fabs(*(Angle+1))>(fabs(*(Angle+1)-360))) *Angle-=360;
}
return true;
}
//根据目标位形矩阵G计算出各关节转角
BOOL CMySDOpenGLView::CoumpterIKinematics(CMatrix g)
{ double tempIAngle1[6],tempIAngle2[6],tempIAngle3[6],tempIAngle4[6];
BOOL F1,F2,F3,F;
int h=0;
F1=true;F2=true;F3=true;F=false;
CMatrix gd(4);
CMatrix gst0(4);
gst0=igst0;
gst0.InvertGaussJordan();
gd=g*gst0;
// CString m_strMatrix3 = gd.ToString(" ");
// AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
// double vq3[4]={q3.x,q3.y,q3.z,1};
// double vq1[3]={q1.x,q1.y,q1.z};
CVector3 r(l2,l0,l1+l3);
CVector3 r1(l2,l0,l1);
CMatrix mq3(4,1);
mq3=Vector3ToMatrix41(&r,1);
CMatrix mq=gd*mq3;
CVector3 tempv;
tempv=MatrixToVector3(4,0,mq);
tempv=tempv-q1;
double delta;
delta=Mag(tempv);
int n;
//||exp(ξ3θ3)q3-q1||=||gst*gst0-1*q3-q1||=δ
F3=InverseKinematics3(&r,&q1,&r1,&omiga3,&delta,&n,tempIAngle3);
for(int i=0;i<n;i++){
CMatrix g3=Makeg3((*(tempIAngle3+i))*PI/180);
CMatrix mp1(4,1);
mp1=Vector3ToMatrix41(&r,1);
CMatrix mp(4,1);
mp=g3*mp1;
mq=gd*mp1;
CVector3 tempp=MatrixToVector3(4,0,mp);
CVector3 tempq=MatrixToVector3(4,0,mq);
int n1;
//exp(ξ1θ1)exp(ξ2θ2)r=gst*gst0-1*r
F2=InverseKinematics2(&tempp,&tempq,&q1,&omiga1,&omiga2,&n1,tempIAngle1,tempIAngle2);
// for(int j=0;j<n1;j++)
for(int k=0;k<n1;k++) {
CMatrix g1=Makeg1((*(tempIAngle1+k))*PI/180);
CMatrix g2=Makeg2((*(tempIAngle2+k))*PI/180);
g3=Makeg3((*(tempIAngle3+i))*PI/180);
g1.InvertGaussJordan();
g2.InvertGaussJordan();
g3.InvertGaussJordan();
CMatrix mq1(4,1);
CVector3 q(1,1,1);
mq1=Vector3ToMatrix41(&q,1);
// CString m_strMatrix3 = mq1.ToString(" ");
// AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
mq=g3*g2*g1*gd*mq1;
// CString m_strMatrix3 = mq.ToString(" ");
// AfxMessageBox(m_strMatrix3, MB_OK|MB_ICONINFORMATION);
tempq=MatrixToVector3(4,0,mq);
//exp(ξ4θ4)q1=exp(-ξ3θ3)exp(-ξ2θ2)exp(-ξ1θ1)gst*gst0-1*q1
F1=InverseKinematics1(&q,&tempq,&q3,&omiga4,tempIAngle4);
if(F1&&F2&&F3) {IAngle1[h]=tempIAngle1[k];
IAngle2[h]=tempIAngle2[k];
IAngle3[h]=tempIAngle3[i];
IAngle4[h]=tempIAngle4[0];
h++;
F=true;
}
}
}
return F;
}
void CMySDOpenGLView::mouse_select(int x,int y)
{
GLuint selectBuff[64];
GLint hits,viewport[4];
GLint render_mode;
glSelectBuffer(64,selectBuff);
glGetIntegerv(GL_VIEWPORT,viewport);
glDisable(GL_LIGHTING);
glMatrixMode(GL_PROJECTION);
glPushMatrix();
glRenderMode(GL_SELECT);
glLoadIdentity();
gluPickMatrix(x,viewport[3]-y,2,2,viewport);
gluPerspective( 45.0f,(GLfloat)w/(GLfloat)h,0.1f, 3000.0f);
// glFrustum(-7,7,-5,5,10,300);
glMatrixMode(GL_MODELVIEW);
//glGetIntegerv(GL_RENDER_MODE, &render_mode);
RenderScence();
//glGetIntegerv(GL_RENDER_MODE, &render_mode);
/*
glInitNames();
glPushName(0);
glLoadName(1);
auxSolidBox(5,5,0.1);
// glPopMatrix();
glFlush();
*/
hits=glRenderMode(GL_RENDER);
glGetIntegerv(GL_RENDER_MODE, &render_mode);
if(hits>0)model_pick=selectBuff[3];
// glPushMatrix();
glMatrixMode(GL_PROJECTION);
glPopMatrix();
glMatrixMode(GL_MODELVIEW);
glEnable(GL_LIGHTING);
}
void CMySDOpenGLView::get_xyz(int x,int y)
{ GLint viewport[4];
GLdouble modelview[16];
GLdouble projection[16];
GLfloat winX,winY,winZ;
// GLdouble object_x,object_y,object_z;
int mouse_x,mouse_y;
mouse_x=x;//鼠标的x坐标
mouse_y=y;//鼠标的y坐标
glGetDoublev(GL_MODELVIEW_MATRIX, modelview);//视图矩阵
glGetDoublev(GL_PROJECTION_MATRIX, projection);//投影矩阵
glGetIntegerv(GL_VIEWPORT, viewport);//视口
winX=(float)mouse_x;//OGL中的窗口x坐标
winY=(float)viewport[3]-(float)mouse_y;//OGL中的窗口y坐标
/*
glReadPixels(mouse_x,//x坐标
int(winY),//y坐标
1,1,//读取一个像素
GL_DEPTH_COMPONENT,//获得深度信息
GL_FLOAT,//数据类型为浮点型
&winZ);//获得的深度值保存在winZ中
*/
gluUnProject((GLdouble)winX,(GLdouble)winY,0.0,modelview,projection,viewport,&near_x,&near_y,&near_z);
//获得OGL中的x,y,z坐标值(GLdouble)winZ
gluUnProject((GLdouble)winX,(GLdouble)winY,1.0,modelview,projection,viewport,&far_x,&far_y,&far_z);
// DrawPoint( object_x, object_y, object_z, 1.0);
}
// draw the rect which indicate the select position
void CMySDOpenGLView::drawRect(float x,float y,float z,float size)
{
glColor3f(1.0f,0.0f,0.0f);
glBegin(GL_QUADS);
glVertex3f(x-size/2.0f,y,z-size/2.0f);
glVertex3f(x-size/2.0f,y,z+size/2.0f);
glVertex3f(x+size/2.0f,y,z+size/2.0f);
glVertex3f(x+size/2.0f,y,z-size/2.0f);
glEnd();
glColor3f(1.0f,1.0f,1.0f);
}
void CMySDOpenGLView::drawLine(GLdouble o_x,GLdouble o_y,GLdouble o_z,GLdouble d_x,GLdouble d_y,GLdouble d_z)
{
glBegin(GL_LINES);
glVertex3f(o_x,o_y,o_z);
glVertex3f(d_x,d_y,d_z);
glEnd();
}
//将三维向量转化为齐次坐标
CMatrix CMySDOpenGLView::Vector3ToMatrix41(CVector3 *vc,int point)
{ double v[4]={vc->x,vc->y,vc->z,point};
CMatrix result(4,1,v);
return result;
}
//将齐次坐标转化为三维向量
CVector3 CMySDOpenGLView::MatrixToVector3(int num,int col,CMatrix m)
{CVector3 result;
double *vector=new double[num];
m.GetColVector(col,vector);
result.x=vector[0];
result.y=vector[1];
result.z=vector[2];
delete vector;
return result;
}
CMatrix CMySDOpenGLView::Makeg1(double cAngle1)
{ double v1[]={cos(cAngle1),0,sin(cAngle1),0,0,1,0,0,-sin(cAngle1),0,cos(cAngle1),0,0,0,0,1};
CMatrix g1(4,v1);
return g1;
}
CMatrix CMySDOpenGLView::Makeg2(double cAngle2)
{ 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};
CMatrix g2(4,v2);
return g2;
}
CMatrix CMySDOpenGLView::Makeg3(double cAngle3)
{ 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};
CMatrix g3(4,v3);
return g3;
}
CMatrix CMySDOpenGLView::Makeg4(double cAngle4)
{ 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};
CMatrix g4(4,v4);
return g4;
}
CMatrix CMySDOpenGLView::Makeg(CVector3 *x,CVector3 *y,CVector3 *z,CVector3 *p)
{double v[]={x->x,y->x,z->x,p->x,x->y,y->y,z->y,p->y,x->z,y->z,z->z,p->z,0,0,0,1};
CMatrix g(4,v);
return g;
}
/*
void CMySDOpenGLView::OnProperties()
{
// TODO: The property sheet attached to your project
// via this function is not hooked up to any message
// handler. In order to actually use the property sheet,
// you will need to associate this function with a control
// in your project such as a menu item or tool bar button.
//
// If mini frame does not already exist, create a new one.
// Otherwise, unhide it
if (m_pPropFrame == NULL)
{
m_pPropFrame = new CDrawPropertyFrame;
CRect rect(0, 0, 0, 0);
CString strTitle;
VERIFY(strTitle.LoadString(IDS_PROPSHT_CAPTION));
if (!m_pPropFrame->Create(NULL, strTitle,
WS_POPUP | WS_CAPTION | WS_SYSMENU, rect, this))
{
delete m_pPropFrame;
m_pPropFrame = NULL;
return;
}
m_pPropFrame->CenterWindow();
}
// Before unhiding the modeless property sheet, update its
// settings appropriately. For example, if you are reflecting
// the state of the currently selected item, pick up that
// information from the active view and change the property
// sheet settings now.
if (m_pPropFrame != NULL && !m_pPropFrame->IsWindowVisible())
m_pPropFrame->ShowWindow(SW_SHOW);
}
*/
void CMySDOpenGLView::OnTxSet()
{
// TODO: Add your command handler code here
// OnProperties();
}
void CMySDOpenGLView::OnUpdateMode(CCmdUI* pCmdUI)
{
// TODO: Add your command update UI handler code here
// OnProperties();
}
void CMySDOpenGLView::OnMode()
{
// TODO: Add your command handler code here
}
⌨️ 快捷键说明
复制代码
Ctrl + C
搜索代码
Ctrl + F
全屏模式
F11
切换主题
Ctrl + Shift + D
显示快捷键
?
增大字号
Ctrl + =
减小字号
Ctrl + -