and here it is the main:
{#using <system.dll>
#include<glut.h>
#include<windows.h>
#include"camera.cpp"
#include"lights.cpp"
#include"ball.cpp"
#include"axises.cpp"
#include"Robot.cpp"
#include"plane.cpp"
#include <stdio.h>
#include <math.h>
float resultArray[2000][3];
int resultCount=0;
int resultIndex=0;
/////////
void MatrixTranspose(double A[3][3], double B[3][3])
{
int i;
int j;
for(i = 0; i < 3; i++)
for(j = 0;j < 3; j++)
A[i][j] = B[j][i];
}
//-----------------------------------------------------------------------------
double Matrix3X3Det(double A[3][3])
{
double inverse_00 = A[1][1]*A[2][2] - A[1][2]*A[2][1];
double inverse_10 = A[1][2]*A[2][0] - A[1][0]*A[2][2];
double inverse_20 = A[1][0]*A[2][1] - A[1][1]*A[2][0];
double det = A[0][0] * inverse_00 +
A[0][1] * inverse_10 +
A[0][2] * inverse_20;
return det;
}
//---------------- inv: A = A^{-1} ---------------------------------------
void MatrixInverse(int z_size, double A[3][3])
{
//int z_size=3; // khodam ezafeeeeeeeeeee kardam???
int i;
int j;
int k;
for (k = 0; k < z_size; k++)
// the pivot element
{
A[k][k] = -1 / A[k][k];
// the pivot column
for (i = 0; i < z_size; i++)
if (i != k) A[i][k] *= A[k][k];
// elements not in a pivot row or column
for (i = 0; i < z_size; i++)
if (i != k)
for (j = 0;j < z_size; j++)
if (j != k)
A[i][j] += A[i][k] * A[k][j];
// elements in a pivot row
for (i = 0; i < z_size; i++)
if (i != k)
A[k][i] *= A[k][k];
}
// change sign
for (i = 0; i < z_size; i++) //reverse sign
for (j = 0; j < z_size; j++)
A[i][j] = -A[i][j];
}
//---------------C = A * B-----------------------------------------------------
void MatrixVectorMul(double C[3], double A[3][3], double B[3])
{
int a;
int b;
C[0]=C[1]=C[2]=0;
for(a=0;a<3;a++)
for(b=0;b<3;b++)
C[a] = C[a] + A[a][b]*B[b];
}
//---------------C = A * B-----------------------------------------------------
void MatrixMatrixMul(double C[3][3], double A[3][3], double B[3][3])
{
int a;
int b;
for(a=0;a<3;a++)
for(b=0;b<3;b++)
C[a][b] = A[a][0]*B[0][b]+A[a][1]*B[1][b]+A[a][2]*B[2][b];
}
//---------------C = A + B-----------------------------------------------------
void MatrixMatrixplus2(double C[3][3], double A[3][3], double B[3][3])
{
int a;
int b;
for(a=0;a<3;a++)
for(b=0;b<3;b++)
C[a][b] = A[a][b]+ B[a][b];//*B[0][b]+A[a][1]*B[1][b]+A[a][2]*B[2][b];
}
//------------- C = A + B (vectror) ----------------------------------------------------------
void MatrixMatrixplus(double C[3], double A[3], double B[3])
{
int a;
for(a=0;a<3;a++)
C[a] = A[a]+ B[a];//*B[0][b]+A[a][1]*B[1][b]+A[a][2]*B[2][b];
}
//------------- C=-A ----------------------------------------------------------
void NegativeMatrix(double C[3], double A[3])
{
int a;
for(a=0;a<3;a++)
C[a]=-A[a];
/void square(int i)
{
int sq = i * i;
cout << "Square of " << i << " is " << sq;
}/
}
/////////////////////////////////////////////////////////////
void Dynamic(double result[], double q[])//, double qdot[3] )//,double KF[3], double Mmdot[3][3])
{
double M[3][3],V[3],G[3];
M[0][0] = 0.25*pow(cos(q[1] + q[2]),2) + cos(q[1] + q[2])*cos(q[1]) + 0.25*(5*pow(cos(q[1]),2)) + 9;
M[0][1] = 0;
M[0][2] = 4;
M[1][0] = 0;
M[1][1] = cos(q[2]) + 8.500000;
M[1][2] = 0.5*cos(q[2]) + 0.250000;
M[2][0] = 4;
M[2][1] = cos(q[2])/2 + 0.250000;
M[2][2] = 4.250000;
V[0] =-(q[3]*(5*q[4]*sin(2*q[1]) + q[4]*sin(2*q[1] + 2*q[2]) + q[5]*sin(2*q[1] + 2*q[2]) + 2*q[5]*sin(q[2]) + 4*q[4]*sin(2*q[1] + q[2]) + 2*q[5]*sin(2*q[1] + q[2])))*0.25;
V[1] =(pow(q[3],2)*sin(2*q[1] + q[2]))*0.5 - (pow(q[5],2)*sin(q[2]))*0.5 + (5*pow(q[3],2)*sin(2*q[1]))*0.125 + (pow(q[3],2)*sin(2*q[1] + 2*q[2]))*0.125 - q[4]*q[5]*sin(q[2]);
V[2] = (pow(q[3],2)*sin(q[2]))*0.25 + (pow(q[4],2)*sin(q[2]))*0.5 + (pow(q[3],2)*sin(2*q[1] + q[2]))*0.25 + (pow(q[3],2)*sin(2*q[1] + 2*q[2]))*0.125;
G[0] = 0;
G[1] = 0.1*(49*cos(q[1] + q[2])) + 0.1*(147*cos(q[1]));
G[2] = 0.1*(49*cos(q[1] + q[2]));
//double a0=1.0000000;double a1=1.0000000;
// double m1=1.0000000;double m2=1.0000000;
//double I0=2.0000000;double I1=3.0000000; double I2=4.0000000;
//double g=9.800000;
//double kz=0.200000;
//// M matrices in the joint space
////notice that M is symmetric
// M[0][0] = I0 + (pow(a1,2)m2pow(cos(q[1] + q[2]),2))/4 + (pow(a0,2)m1pow(cos(q[1]),2))/4 + pow(a0,2)m2pow(cos(q[1]),2) + a0a1m2*cos(q[1] + q[2])*cos(q[1]);//sin(q[1])
//M[0][1] = 0;
//M[0][2] = 0;
//M[1][0] = 0;
//M[1][1] = I1 + (pow(a0,2)*m1)/4 + pow(a0,2)*m2 + (pow(a1,2)*m2)/4 + a0*a1*m2*cos(q[2]);//5.0000*cos(q[2])
//M[1][2] = (a1*m2*(a1 + 2*a0*cos(q[2])))/4;
//M[2][0] = 0;
//M[2][1] = (a1*m2*(a1 + 2*a0*cos(q[2])))/4;//q[1]
//M[2][2] = (m2*pow(a1,2))/4 + I2;//pai5;
////notice that C is skew-symmetric
//V[0] = -(q[3]*(pow(a0,2)*m1*q[4]*sin(2*q[1]) + 4*pow(a0,2)*m2*q[4]*sin(2*q[1]) + pow(a1,2)*m2*q[4]*sin(2*q[1] + 2*q[2]) + pow(a1,2)*m2*q[5]*sin(2*q[1] + 2*q[2]) + 4*a0*a1*m2*q[4]*sin(2*q[1] + q[2]) + 2*a0*a1*m2*q[5]*sin(2*q[1] + q[2]) + 2*a0*a1*m2*q[5]*sin(q[2])))/4;//3.0000*q[5]*sin(q[0]);
//V[1] = (pow(a0,2)*m1*pow(q[3],2)*sin(2*q[1]))/8 + (pow(a0,2)*m2*pow(q[3],2)*sin(2*q[1]))/2 + (pow(a1,2)*m2*pow(q[3],2)*sin(2*q[1] + 2*q[2]))/8 - (a0*a1*m2*pow(q[5],2)*sin(q[2]))/2 + (a0*a1*m2*pow(q[3],2)*sin(2*q[1] + q[2]))/2 - a0*a1*m2*q[4]*q[5]*sin(q[2]);//3.0000*sin(q[2 ]);
//V[2] =(a1*m2*(a1*pow(q[3],2)*sin(2*q[1] + 2*q[2]) + 2*a0*pow(q[3],2)*sin(q[2]) + 4*a0*pow(q[4],2)*sin(q[2]) + 2*a0*pow(q[3],2)*sin(2*q[1] + q[2])))/8;//2.0000;
//G[0] = 0;
//G[1] = g*m2*((a1*cos(q[1] + q[2]))/2 + a0*cos(q[1])) + (a0*g*m1*cos(q[1]))/2;//2.0000*cos(q[2]);
//G[2] = (a1*g*m2*cos(q[1] + q[2]))/2;//3.0000;
// M matrices in the joint space
//notice that M is symmetric
////////adadi
//M[0][0]= pow(cos(q[1] + q[2]),2)/4 + cos(q[1] + q[2])cos(q[1]) + (5pow(cos(q[1]),2))/4 + 2;
//M[0][1]= 0;
//M[0][2]= 0;
//
//M[1][0]= 0;
//M[1][1]= cos(q[2]) + 9/2;
//M[1][2]= cos(q[2])/2 + 1/4;
//
//M[2][0]= 0;
//M[2][1]= cos(q[2])/2 + 1/4;
//M[2][2]= 17/4;
//
////notic
//V[0] = -(q[3](5q[4]sin(2q[1]) + q[4]sin(2q[1] + 2q[2]) + q[5]sin(2q[1] + 2q[2]) + 2q[5]sin(q[2]) + 4q[4]sin(2q[1] + q[2]) + 2q[5]sin(2q[1] + q[2])))/4;
//V[1] = (pow(q[3],2)sin(2q[1] + q[2]))/2 - (pow(q[5],2)sin(q[2]))/2 + (5pow(q[3],2)sin(2q[1]))/8 + (pow(q[3],2)sin(2q[1] + 2*q[2]))/8 - q[4]q[5]sin(q[2]);
//V[2] = (pow(q[3],2)sin(q[2]))/4 + (pow(q[4],2)sin(q[2]))/2 + (pow(q[3],2)sin(2q[1] + q[2]))/4 + (pow(q[3],2)sin(2q[1] + 2q[2]))/8;
//
//
//G[0] = 0;
//G[1] = (49cos(q[1] + q[2]))/10 + (147cos(q[1]))/10;
//G[2] = (49cos(q[1] + q[2]))/10;
////////////////////random example
// M[0][0] = sin(q[1]);//pai1+pai2SQ(cos(Th[1]))+(pai3+pai5)SQ(sin(Th[2]))+2pai6cos(Th[1])*sin(Th[2]);
//M[0][1] = 2.000000;
//M[0][2] = 5.000000;
//M[1][0] = 7.000000;
//M[1][1] = 5.000000*cos(q[2]);//pai4+pai5-2*pai6*sin(Th[1]-Th[2]);
//M[1][2] = 4.000000;//pai5-pai6*sin(Th[1]-Th[2]);
//M[2][0] = 0.000000;
//M[2][1] = q[1];//pai5-pai6*sin(Th[1]-Th[2]);
//M[2][2] = 2.000000;//pai5;
////notice that C is skew-symmetric
//V[0] = 3.000000*q[5]*sin(q[0]);//-Thdot[1]*(pai2*sin(Th[1])*cos(Th[1])+pai6*sin(Th[1])*sin(Th[2]))+Thdot[2]*((pai3+pai5)*sin(Th[2])*cos(Th[2])+pai6*cos(Th[1])*cos(Th[2]));
//V[1] = 3.000000*sin(q[2]);//-Thdot[0]*(pai2*sin(Th[1])*cos(Th[1])+pai6*sin(Th[1])*sin(Th[2]));
//V[2] =2.000000;// Thdot[0]*((pai3+pai5)*sin(Th[2])*cos(Th[2])+pai6*cos(Th[1])*cos(Th[2]));
//G[0] = 0.000000;
//G[1] = 2.000000*cos(q[2]);//pai7*cos(Th[1]);
//G[2] = 3.000000;//pai8*sin(Th[2]);
///////////////////////
double r[3] = {0,0,0};
MatrixMatrixplus(r, V, G);
double s[3] = {0,0,0};
MatrixInverse(3,M);
MatrixVectorMul(s, M, r);
NegativeMatrix(result, s);
}
//Example: y1’=y2+y3-3y1, y2’=y1+y3-3y2, y3’=y1+y2-3y3
double fp(int k, double x, double *y) {
double result[3];
Dynamic(result,y);
switch(k) {
case 0: return y[3];
case 1: return y[4];
case 2: return y[5];
case 3: return result[0];
case 4: return result[1];
case 5: return result[2];
default: return 0;
}
}
// print results
void Display(double *t1,double *t2,double *t3,double *t4,double *t5,double *t6,int m,int p,double xi,double xf) {
int i;
double h,x;
h=(xf-xi)/(m-1);
x=xi-h;
printf("
X");
for (i=1; i<=p; i++) printf(" Y%d", i);
printf("
“);
printf(”-------------------------------------------------------------------------------
");
resultCount=m;
for (i=1; i<m+1; i++) {
x += h;
resultArray[i][0]=57.295780t1[i];
resultArray[i][1]=57.295780t2[i];
resultArray[i][2]=57.295780t3[i];
printf(" %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f
",x,57.295780t1[i],57.295780t2[i],57.295780t3[i],57.295780t4[i],57.295780t5[i],57.295780*t6[i]);
}
// resultArray=new Point3D[m];
// resultCount=m;
// for (i=1; i<m+1; i++) {
// x += h;
//resultArray[i]=Point3D(t1[i],t2[i],t3[i]);
//printf(" %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f %9.6f
“,x,t1[i],t2[i],t3[i],t4[i],t5[i],t6[i]);
// }
printf(”--------------------------------------------------------------------------------
");
}
void Eqdifp(double *t1,double *t2,double *t3,double *t4,double *t5,double *t6,double xi,double xf,double *Yi,
int m,int p,int fi)
{
double h,x;
double ta[10],tb[10],tc[10],td[10],y[10],z[10];
int i,j,k,ni;
if (fi<1) return;
h = (xf - xi) / fi / (m-1);
p–;
t1[1]=Yi[0];
t2[1]=Yi[1];
t3[1]=Yi[2];
t4[1]=Yi[3];
t5[1]=Yi[4];
t6[1]=Yi[5];
for (k=0; k<p+1; k++) {
y[k]=Yi[k]; z[k]=Yi[k];
}
for (i=1; i<m+1; i++) {
ni=(i-1)fi-1;
for (j=1; j<fi+1; j++) {
x=xi+h(ni+j); // baraye chieeeeeeeeeeeeeeeeeeeeeeeeeee?
for (k=0; k<p+1; k++) y[k]=z[k];
for (k=0; k<p+1; k++) ta[k]=hfp(k,x,y);
for (k=0; k<p+1; k++) y[k]=z[k]+ta[k]/2;
x=x+h/2;
for (k=0; k<p+1; k++) tb[k]=hfp(k,x,y);
for (k=0; k<p+1; k++) y[k]=z[k]+tb[k]/2;
for (k=0; k<p+1; k++) tc[k]=hfp(k,x,y);
for (k=0; k<p+1; k++) y[k]=z[k]+tc[k];
x=x+h/2;
for (k=0; k<p+1; k++) td[k]=hfp(k,x,y);
for (k=0; k<p+1; k++)
z[k]=z[k]+(ta[k]+2tb[k]+2tc[k]+td[k])/6;
}
t1[i+1]=z[0];
t2[i+1]=z[1];
t3[i+1]=z[2];
t4[i+1]=z[3];
t5[i+1]=z[4];
t6[i+1]=z[5];
}
Display(t1,t2,t3,t4,t5,t6,m,p+1,xi,xf);
}
////////
Camera c;
Lights ls;
Ball b(.13);
Axises ax;
Robot r(.05,.5);
Plane pl;
int bi=0,bi1=0;
void reshape(int x, int y)
{
// r=*new Robot(float b,float a,float c,float d,float e,float f)//base- arm lenght- q1-q2-q3-q hand
//if (y == 0 || x == 0) return; //Nothing is visible then, so return
//Set a new projection matrix
glMatrixMode(GL_PROJECTION);
glLoadIdentity();
//Angle of view:40 degrees
//Near clipping plane distance: 0.5
//Far clipping plane distance: 30.0
gluPerspective(40.0,(GLdouble)x/(GLdouble)y,0.5,30);
glMatrixMode(GL_MODELVIEW);
glViewport(0,0,x,y); //Use the whole window for rendering
}
int i=0;
void Display(void)
{
//////////////////////////////////
glClear(GL_COLOR_BUFFER_BIT|GL_DEPTH_BUFFER_BIT);
glLoadIdentity();
c.Render();
ls.Render();
glColor3f(.8,.8,.8);
//w.Draw();
r.Draw();
ax.Draw();
pl.Draw();
b.Draw();
ls.DrawDirections();
glutSwapBuffers();
}
int h=0;
int y;
int th=0;
void KeyDown(unsigned char key, int x, int y)
{
if(key=='1')
{
r.SetTetaArm1(i++);
}
if(key=='2')
{
r.SetTetaArm1(i--);
}
if(key=='3')
{
r.SetTetaArm2(bi++);
}
if(key=='4')
{
r.SetTetaArm2(bi--);
}
/*if(key=='3')
{
r.SetTetaHand(th++);
}*/
if(key=='b')
{
r.SetTetaBase(bi1++);
}
if(key==27)
PostQuitMessage(0);
switch(h)//-y)
{
case 0:
c.Do(key);
break;
case 1:
ls.Do(key);
break;
case 2:
b.Do(key);
break;
default:
//w.Do(h,key);
break;
}
glutPostRedisplay();
}
void myinit()
{
glBlendFunc(GL_SRC_ALPHA, GL_ONE_MINUS_SRC_ALPHA);
glEnable(GL_BLEND);
glEnable(GL_LINE_SMOOTH);
glShadeModel(GL_SHADE_MODEL);
glEnable(GL_BLEND);
glEnable(GL_DEPTH_TEST);
glEnable(GL_NORMALIZE);
glEnable(GL_LIGHTING);
glEnable(GL_COLOR_MATERIAL);
ls.init();
glLightModeli(GL_LIGHT_MODEL_TWO_SIDE,GL_FALSE);
glLightModeli(GL_LIGHT_MODEL_LOCAL_VIEWER,GL_TRUE);
r.SetBall(&b);
}
void menu(int ch)
{
h=ch;
}
void createMenu()
{
int i=0;
glutCreateMenu(menu);
glutAddMenuEntry(“Camera”,i++);
glutAddMenuEntry(“Lights”,i++);
glutAddMenuEntry(“Ball”,i++);
glutAttachMenu(GLUT_RIGHT_BUTTON);
}
void OnTimedEvent( System::Object^ source, System::Timers::ElapsedEventArgs^ e )
{
resultIndex++;
if(resultIndex>=resultCount)
{
((System::Timers::Timer^)source)->Enabled = false;
return;
}
r.SetTetaBase(resultArray[resultIndex][0]);
r.SetTetaArm1(resultArray[resultIndex][1]);
r.SetTetaArm2(resultArray[resultIndex][2]);
//cout<<"
x of base is"<<r.targetBase.getX()<<endl;
//cout<<"
x of Arm1 is"<<r.targetArm1.getX()<<endl;
//cout<<"
x of base is"<<r.targetArm2.getX()<<endl;
//r.SetTetaBase(resultArray[resultIndex].GetX());
//r.SetTetaArm1(resultArray[resultIndex].GetY());
//r.SetTetaArm2(resultArray[resultIndex].GetZ());
glutPostRedisplay();
}
//cout<<r.targetHand.getX();
/////////////////////////////////////////////////////
#define SIZE 2000 //baraye chieeeeeeeeeeeeee?
int fi,ndata,p=6.00;
double xi,xf;
double yi[10];
double pi=3.1416;
double v1[SIZE],v2[SIZE],v3[SIZE],v4[SIZE],v5[SIZE],v6[SIZE]; //baraye chieeeeeeeeeeeeeee?
/////////////////////////////////////////////////////
int main(int argc, char **argv)
{
/////////////////////////////////////////////////////
printf("
DIFFERENTIAL EQUATIONS WITH 6 VARIABLES OF ORDER 1
“);
printf(” of type yi’ = f(y1,y2,…,yn), i=1…n
");
xi=0;
xf=5;
printf("
end value x : %f ",xf);
i=0; yi[i]=0;
i++;yi[i]=-1.570796;
i++;yi[i]=-1.570796;
i++;yi[i]=0;
i++;yi[i]=0;
i++;yi[i]=0;
ndata=501; fi=1;
// printf("
begin value x : “);
// scanf(”%lf",&xi);
// printf(" end value x : “);
// scanf(”%lf",&xf);
// for (int i=0; i<p; i++) {
// printf(" y%d value at x0 : “,i+1);
//scanf(”%lf",&yi[i]);
// }
// printf(" number of points : “);
// scanf(”%d",&ndata);
// printf(" finesse : “);
// scanf(”%d",&fi);
Eqdifp(v1,v2,v3,v4,v5,v6,xi,xf,yi,ndata,p,fi);
/////////////////////////////////////////////////////
glutInit(&argc, argv);
glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGBA | GLUT_DEPTH);
glutInitWindowSize(900,500);
int u=glutCreateWindow(“3DOF robot”);
myinit();
createMenu();
glutDisplayFunc(Display);
glutReshapeFunc(reshape);
glutKeyboardFunc(KeyDown);
System::Timers::Timer^ aTimer = gcnew System::Timers::Timer( 10 );
//tt=aTimer;
// Hook up the Elapsed event for the timer.
aTimer->Elapsed += gcnew System::Timers::ElapsedEventHandler( OnTimedEvent );
// Set the Interval to 2 seconds (2000 milliseconds).
aTimer->Enabled = true;
glutMainLoop();
return 0;
}
}