API tools faq
paste
dorbird

Inverse Kinematics Krane OpenGL C++

dorbird
Jun 9th, 2015
408
0
Never
Add comment
Not a member of Pastebin yet? Sign Up, it unlocks many cool features!
C++ 13.74 KB | None | 0 0
raw download clone embed print report
  1. /*
  2.     Benötigt matrix.h erhältlich hier http://matrix.drque.net/
  3.     Tastenbelegung:
  4.         W,S,A,D,Q,E - Steuerung der Kugel/Ziel-punkt
  5.         X,Y,Z - Kameraausrichtung auf XY,YZ und ZX Ebenen(Front-, Seiten- und Draufsicht)
  6.         O,P - Punkte menge durchlaufen
  7.         1,2,3,4 Armlänge ändern
  8.         i - start/stop animate
  9.         u - richtung der animation
  10.  
  11. */
  12.  
  13. #define GLFW_DLL
  14. #include <GLFW/glfw3.h>
  15. #include <stdlib.h>
  16. #include <stdio.h>
  17. #include <math.h>
  18. #include <iostream>
  19.  
  20. using namespace std;
  21.  
  22. #include "vec3.hpp"
  23. #include "matrix.h"
  24.  
  25. static double alpha_ = 0;
  26. static double beta_ = 0;
  27. static double gamma_= 0;
  28. static double delta_ = 0;
  29. static double epsilon_ = 0;
  30. static double zetta_ = 0;
  31. static double bla_ = 0;
  32. static double dt_ = 1;
  33. static double angleXAxis_=0;
  34. static double angleYAxis_=-90;
  35. static double angleZAxis_=0;
  36.  
  37. static double a[2] = {15,15};
  38. Vec3 p( 5, 0, 5);
  39. Vec3 q(0,0,0);
  40. Vec3 theta(M_PI/2, M_PI/2, M_PI/2);
  41. //static double q[3] = {0, 0, 0};
  42. int counter=0;
  43. bool animate;
  44. bool richtung;
  45.  
  46. double J[3][3];
  47.  
  48.  
  49.  
  50. static Vec3 mouseSpeed(0,0,0);
  51.  
  52. static double window_width_ = 1000;
  53. static double window_height_ =1000;
  54. static double zoomIn = 1;
  55.  
  56. vector<Vec3> getTorus(double radius1,double radius2,int resolution1,int resolution2){
  57.     vector<Vec3>punkte;
  58.     for (int i=0;i<=resolution1;i++){
  59.         double alpha=i*2*M_PI/resolution1;
  60.         for (int j=0;j<=resolution2;j++){
  61.             double beta=j*2*M_PI/resolution2;
  62.             punkte.push_back(Vec3((radius1+radius2*cos(beta))*cos(alpha),(radius1+radius2*cos(beta))*sin(alpha),radius2*sin(beta)));
  63.         }
  64.     }
  65.     return punkte;
  66. }
  67.  
  68.  
  69. vector<Vec3> getBall(double size,int slices,double resolution){
  70.     vector<Vec3>punkte;
  71.     for (int i=-slices;i<=slices;i++){
  72.         double aktslice=i*size/slices;
  73.         double R=sqrt(pow(size,2)-pow(aktslice,2));
  74.         for (int j=0;j<=resolution;j++){
  75.             double aktPunkt=j*2*M_PI/resolution;
  76.             punkte.push_back(Vec3(R*cos(aktPunkt),R*sin(aktPunkt),aktslice));
  77.         }
  78.     }
  79.     return punkte;
  80. }
  81.  
  82. void SetMaterialColor(int side, double r, double g, double b) {
  83.   float amb[4], dif[4], spe[4];
  84.   int mat;
  85.  
  86.   dif[0] = r;
  87.   dif[1] = g;
  88.   dif[2] = b;
  89.  
  90.   for(int i = 0; i < 3; i++) {
  91.     amb[i] = .1 * dif[i];
  92.     spe[i] = .5;
  93.   }
  94.   amb[3] = dif[3] = spe[3] = 1.0;
  95.  
  96.   switch(side){
  97.     case 1:     mat = GL_FRONT;
  98.       break;
  99.     case 2:     mat = GL_BACK;
  100.       break;
  101.     default: mat = GL_FRONT_AND_BACK;
  102.   }
  103.  
  104.   glMaterialfv(mat, GL_AMBIENT, amb);
  105.   glMaterialfv(mat, GL_DIFFUSE, dif);
  106.   glMaterialfv(mat, GL_SPECULAR, spe);
  107.   glMaterialf( mat, GL_SHININESS, 20);
  108. }
  109.  
  110. void DrawQuad(Vec3 a,Vec3 b,Vec3 c,Vec3 d){
  111.         Vec3 norm((a-b)%(a-c));
  112.         norm=norm/norm.Length();
  113.         glBegin(GL_QUADS);
  114.         glNormal3dv(norm.p);
  115.         glVertex3dv(a.p);
  116.         glVertex3dv(b.p);
  117.         glVertex3dv(c.p);
  118.         glVertex3dv(d.p);
  119.         glEnd();
  120. }
  121. void DrawTriangle(Vec3 a,Vec3 b,Vec3 c){
  122.         Vec3 norm((a-b)%(a-c));
  123.         norm=norm/norm.Length();
  124.         glBegin(GL_TRIANGLES);
  125.         glNormal3dv(norm.p);
  126.         glVertex3dv(a.p);
  127.         glVertex3dv(b.p);
  128.         glVertex3dv(c.p);
  129.         glEnd();
  130. }
  131.  
  132. void DrawZylinder(double radius,double hoehe,double res){
  133.         int i;
  134.         double a1=2* M_PI /res;
  135.         Vec3 M1(0,0,0);
  136.         Vec3 M2(0,0,hoehe);
  137.         for(i=0; i<=res; i++){
  138.                 Vec3 P0(radius*sin(a1*(i+0)),radius*cos(a1*(i+0)),hoehe);
  139.                 Vec3 P1(radius*sin(a1*(i+0.5)),radius*cos(a1*(i+0.5)),0);
  140.                 Vec3 P2(radius*sin(a1*(i+1)),radius*cos(a1*(i+1)),hoehe);
  141.                 Vec3 P3(radius*sin(a1*(i+1.5)),radius*cos(a1*(i+1.5)),0);
  142.                 DrawTriangle(P0,P1,P2);
  143.                 DrawTriangle(P1,P2,P3);
  144.                 DrawTriangle(P0,M2,P2);
  145.                 DrawTriangle(P3,P1,M1);
  146.         }
  147. }
  148.  
  149. // draw a sphere composed of triangles
  150. void DrawSphere(const Vec3& ctr, double r){
  151.   int     i, j,
  152.           n1 = 20, n2 = 60;
  153.   Vec3    normal, v1;
  154.   double  a1, a1d = M_PI / n1,
  155.           a2, a2d = M_PI / n2,
  156.           s1, s2,
  157.           c1, c2;
  158.  
  159.   glShadeModel(GL_SMOOTH);
  160.   for(i = 0; i < n1; i++){
  161.     a1 = i * a1d;
  162. //    if(i<=n1*0.5)SetMaterialColor(3,0,0,0);
  163. //    if(i>n1*0.5)SetMaterialColor(3,1,1,1);
  164.  
  165.     glBegin(GL_TRIANGLE_STRIP);
  166.     for(j = 0; j <= n2; j++){
  167.       a2 = (j + .5 * (i % 2)) * 2 * a2d;
  168.  
  169.       s1 = sin(a1);
  170.       c1 = cos(a1);
  171.       s2 = sin(a2);
  172.       c2 = cos(a2);
  173.       normal = c1 * XVec3 + s1 * (c2 * YVec3 + s2 * ZVec3);
  174.       v1 = ctr + r * normal;
  175.       glNormal3dv(normal.p);
  176.       glVertex3dv(v1.p);
  177.  
  178.       s1 = sin(a1 + a1d);
  179.       c1 = cos(a1 + a1d);
  180.       s2 = sin(a2 + a2d);
  181.       c2 = cos(a2 + a2d);
  182.       normal = c1 * XVec3 + s1 * (c2 * YVec3 + s2 * ZVec3);
  183.       v1 = ctr + r * normal;
  184.       glNormal3dv(normal.p);
  185.       glVertex3dv(v1.p);
  186.     }
  187.     glEnd();
  188.   }
  189. }
  190.  
  191. void DrawCube(double seitenlaenge){
  192.         //Eckpunkte
  193.         Vec3 P1(-seitenlaenge/2,seitenlaenge/2,-seitenlaenge/2);
  194.         Vec3 P2(seitenlaenge/2,seitenlaenge/2,-seitenlaenge/2);
  195.         Vec3 P3(seitenlaenge/2,seitenlaenge/2,seitenlaenge/2);
  196.         Vec3 P4(-seitenlaenge/2,seitenlaenge/2,seitenlaenge/2);
  197.         Vec3 P5(-seitenlaenge/2,-seitenlaenge/2,-seitenlaenge/2);
  198.         Vec3 P6(seitenlaenge/2,-seitenlaenge/2,-seitenlaenge/2);
  199.         Vec3 P7(seitenlaenge/2,-seitenlaenge/2,seitenlaenge/2);
  200.         Vec3 P8(-seitenlaenge/2,-seitenlaenge/2,seitenlaenge/2);
  201.         //Seiten
  202.         DrawQuad(P1,P2,P3,P4);
  203.         DrawQuad(P3,P2,P6,P7);
  204.         DrawQuad(P4,P3,P7,P8);
  205.         DrawQuad(P1,P4,P8,P5);
  206.         DrawQuad(P2,P1,P5,P6);
  207.         DrawQuad(P5,P6,P7,P8);
  208. }
  209.  
  210. // set viewport transformations and draw objects
  211. void InitLighting() {
  212.     GLfloat lp1[4]  = { 10,  20,  10,  0};
  213.     GLfloat lp2[4]  = { -50,  50, -10,  0};
  214.     GLfloat lp3[4]  = { -5,  -50, -10,  0};
  215.     GLfloat red[49]  = {1.0, .8,  .8,  1};
  216.     GLfloat blue[4] = { .8, .8, 1.0,  1};
  217.    
  218.     glEnable(GL_DEPTH_TEST);
  219.     glDepthFunc(GL_LESS);
  220.     glClearColor(1, 1, 1, 1);
  221.     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  222.    
  223.     glShadeModel(GL_SMOOTH);
  224.     glLightModeli(GL_LIGHT_MODEL_TWO_SIDE, 1);
  225.     glEnable(GL_LIGHTING);
  226.    
  227.     glLightfv(GL_LIGHT1, GL_POSITION, lp1);
  228.     glLightfv(GL_LIGHT1, GL_DIFFUSE,  red);
  229.     glLightfv(GL_LIGHT1, GL_SPECULAR, red);
  230.     glEnable(GL_LIGHT1);
  231.    
  232.     glLightfv(GL_LIGHT2, GL_POSITION, lp2);
  233.     glLightfv(GL_LIGHT2, GL_DIFFUSE,  blue);
  234.     glLightfv(GL_LIGHT2, GL_SPECULAR, blue);
  235.     glEnable(GL_LIGHT2);
  236.    
  237.     glLightfv(GL_LIGHT2, GL_POSITION, lp3);
  238.     glLightfv(GL_LIGHT2, GL_DIFFUSE,  blue);
  239.     glLightfv(GL_LIGHT2, GL_SPECULAR, blue);
  240.     glEnable(GL_LIGHT3);
  241.    
  242.     glClearColor(1, 0, 0, 1);
  243.     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  244.    
  245.     // init viewport to canvassize
  246.     glViewport(0, 0, window_width_, window_height_);
  247.    
  248.     // init coordinate system
  249.     glMatrixMode(GL_PROJECTION);
  250.     glLoadIdentity();
  251.     glOrtho(-30*zoomIn, 30*zoomIn, -30*zoomIn, 30*zoomIn, -50*zoomIn, 50*zoomIn);
  252.    
  253.     //  double n=10;
  254.     //  double f=40;
  255.     //  double a=0.5;
  256.     //  double fov=90;
  257.     //  double l=-1*n*tan((a*fov)/2);
  258.     //  double r=f*tan((a*fov)/2);
  259.     //  double b=-1*n*tan(fov/2);
  260.     //  double t=f*tan(fov/2);
  261.     //
  262.     //  glFrustum(l, r, b, t, n, f);
  263.    
  264.     glMatrixMode(GL_MODELVIEW);
  265.     glLoadIdentity();
  266. }
  267.  
  268.  
  269.  
  270. void DrawKrane(){
  271.     double radius=1;
  272.     glPushMatrix();
  273.     glRotated(180+(180/M_PI) * theta.p[2],0,0,1);
  274.    
  275.     glRotated(-90+(180/M_PI) * theta.p[0],0,1,0);
  276.     DrawSphere(Vec3(0,0,0),radius);
  277.     DrawZylinder(radius,a[0],50);
  278.     glTranslated(0,0,a[0]);
  279.     glRotated((180/M_PI) * theta.p[1],0,1,0);
  280.     DrawSphere(Vec3(0,0,0),radius);
  281.     DrawSphere(Vec3(0,0,a[1]),radius);
  282.     DrawZylinder(radius,a[1],50);
  283.    
  284.     glPopMatrix();
  285. }
  286.  
  287. vector<Vec3>Punkte=getTorus(10,5,100,100);
  288. void IK(){
  289.     for(int i = 0; i <= 5; i++){
  290.         Vec3 r=a[0]*Vec3(cos(theta.p[0]) * cos(theta.p[2]),
  291.                             cos(theta.p[0]) * sin(theta.p[2]),
  292.                             sin(theta.p[0]));
  293.  
  294.         Vec3 s=a[1]*Vec3(cos(theta.p[0] + theta.p[1]) * cos(theta.p[2]),
  295.                             cos(theta.p[0] + theta.p[1]) * sin(theta.p[2]),
  296.                             sin(theta.p[0] + theta.p[1]));
  297.  
  298.         Vec3 f = q + r + s;
  299.  
  300.         Vec3 F_theta1(-a[0] * sin(theta.p[0]) * cos(theta.p[2]) - a[1] * sin(theta.p[0] + theta.p[1]) * cos(theta.p[2]),
  301.                         -a[0] * sin(theta.p[0]) * sin(theta.p[2]) - a[1] * sin(theta.p[0] + theta.p[1]) * sin(theta.p[2]),
  302.                         a[0] * cos(theta.p[0]) + a[1] * cos(theta.p[0] + theta.p[1]));
  303.  
  304.         Vec3 F_theta2(-a[1] * sin(theta.p[0] + theta.p[1]) * cos(theta.p[2]),
  305.                         -a[1] * sin(theta.p[0] + theta.p[1]) * sin(theta.p[2]),
  306.                         a[1] * cos(theta.p[0] + theta.p[1]));
  307.  
  308.         Vec3 F_theta3(-a[0]*cos(theta.p[0]) *sin(theta.p[2]) - a[1]*cos(theta.p[0] + theta.p[1] ) * sin(theta.p[2]),
  309.                         a[0]*cos(theta.p[0]) *cos(theta.p[2]) + a[1]*cos(theta.p[0] + theta.p[1] ) * cos(theta.p[2]),
  310.                         0);
  311.  
  312.         for(int i=0;i<=2;i++){
  313.             J[i][0] = F_theta1.p[i];
  314.             J[i][1] = F_theta2.p[i];
  315.             J[i][2] = F_theta3.p[i];
  316.         }
  317.  
  318.  
  319.         Matrix<double> Jacobi(3,3);
  320.         for(int k=0;k<=2;k++){
  321.             for(int l=0;l<=2;l++){
  322.                 Jacobi.put(k,l,J[k][l]);
  323.             }
  324.         }
  325.  
  326.         Matrix<double> JacobiRes(3,3);
  327.         JacobiRes = (Jacobi.getTranspose() * Jacobi).getInverse() * Jacobi.getTranspose();
  328.  
  329.         Vec3 dtheta(0,0,0);
  330.         for(int i = 0; i < 3; i++){
  331.                 for(int j = 0; j < 3; j++){
  332.                         dtheta.p[i]+=JacobiRes.get(i,j)*(p-f).p[j];
  333.                 }
  334.         }
  335.         theta = theta + dtheta;
  336.     }
  337. }
  338.  
  339.  
  340. // draw the entire scene
  341. void Preview() {
  342.     glMatrixMode(GL_MODELVIEW);
  343.     glLoadIdentity();                                                 // Reset The Current Modelview Matrix
  344.     glTranslated(0, 0, -30.0);      // Move 10 units backwards in z,
  345.                                   // since camera is at origin
  346.  
  347.     //glPushMatrix();
  348.     glRotated(angleXAxis_,0,1,0);
  349.     glRotated(angleYAxis_,1,0,0);
  350.     glRotated(angleZAxis_,0,0,1);
  351.     SetMaterialColor(3, 1, 0.5, 0);
  352.     for(int g=0;g<counter;g++){
  353.     //for(int g=0;g<(int)Punkte.size();g++){
  354.         Vec3 p1=Punkte.at(g);
  355.         glPushMatrix();
  356.         glTranslated(p1.p[0],p1.p[1],p1.p[2]);
  357.         DrawCube(0.2);
  358.         glPopMatrix();
  359.     }
  360.       if(animate){
  361.           if (counter<Punkte.size()-1 && !richtung){
  362.               p=Punkte.at(counter);
  363.               counter++;}
  364.           if(counter>0 && richtung){
  365.               p=Punkte.at(counter);
  366.               counter--;
  367.           }
  368.       }
  369.  
  370.     IK();
  371.     DrawKrane();
  372.     SetMaterialColor(3, 1, 0, 0);
  373.     DrawSphere(p, 1.1);
  374.     glPopMatrix();
  375.  
  376. }
  377. void mouse_button_callback(GLFWwindow* window, int button, int action, int mods)
  378. {
  379.     if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_PRESS){
  380.        double xpos, ypos;
  381.        glfwGetCursorPos(window, &xpos, &ypos);
  382.        mouseSpeed.p[0] = xpos;
  383.        mouseSpeed.p[2] = ypos;
  384.     }
  385.     if (button == GLFW_MOUSE_BUTTON_LEFT && action == GLFW_RELEASE){
  386.        double xpos, ypos;
  387.        glfwGetCursorPos(window, &xpos, &ypos);
  388.        mouseSpeed.p[0] = xpos - mouseSpeed.p[0];
  389.        mouseSpeed.p[2] = ypos - mouseSpeed.p[2];
  390.     }
  391.  
  392.  
  393. }
  394. void key_callback(GLFWwindow* window, int key, int scancode, int action, int mods){
  395.     if (key == GLFW_KEY_N)dt_+=0.1;
  396.     if (key == GLFW_KEY_M)dt_-=0.1;
  397.     if (key == GLFW_KEY_UP)angleYAxis_+=dt_;
  398.     if (key == GLFW_KEY_DOWN)angleYAxis_-=dt_;
  399.     if (key == GLFW_KEY_LEFT)angleXAxis_+=dt_;
  400.     if (key == GLFW_KEY_RIGHT)angleXAxis_-=dt_;
  401.  
  402.     if (key == GLFW_KEY_J){
  403.         zoomIn += 0.01;
  404.     }
  405.  
  406.     if (key == GLFW_KEY_K){
  407.         zoomIn -= 0.01;
  408.     }
  409.  
  410.  
  411.     if (key == GLFW_KEY_O){
  412.         if(counter<Punkte.size()-1){
  413.               p=Punkte.at(counter);
  414.               counter++;
  415.         }
  416.     }
  417.     if (key == GLFW_KEY_P){
  418.           if(counter>0){
  419.               p=Punkte.at(counter);
  420.               counter--;
  421.           }
  422.     }
  423.     if (key == GLFW_KEY_I && action == GLFW_PRESS){
  424.         animate = !animate;
  425.     }
  426.     if (key == GLFW_KEY_U && action == GLFW_PRESS){
  427.         richtung = !richtung;
  428.     }
  429.  
  430.  
  431.  
  432.     if (key == GLFW_KEY_Y){
  433.         angleXAxis_=0;
  434.         angleYAxis_=-90;
  435.         angleZAxis_=0;
  436.     }
  437.     if (key == GLFW_KEY_Z){
  438.         angleXAxis_=0;
  439.         angleYAxis_=0;
  440.         angleZAxis_=0;
  441.     }
  442.     if (key == GLFW_KEY_X){
  443.         angleXAxis_=0;
  444.         angleYAxis_=-90;
  445.         angleZAxis_=-90;
  446.     }
  447.  
  448.     if (key == GLFW_KEY_W)p.p[2] +=dt_;
  449.     if (key == GLFW_KEY_S)p.p[2] -=dt_;
  450.     if (key == GLFW_KEY_A)p.p[0] -=dt_;
  451.     if (key == GLFW_KEY_D)p.p[0] +=dt_;
  452.     if (key == GLFW_KEY_Q)p.p[1] +=dt_;
  453.     if (key == GLFW_KEY_E)p.p[1] -=dt_;
  454.  
  455.     if (key == GLFW_KEY_1)a[0]+=dt_;
  456.     if (key == GLFW_KEY_2)a[0]-=dt_;
  457.     if (key == GLFW_KEY_3)a[1]+=dt_;
  458.     if (key == GLFW_KEY_4)a[1]-=dt_;
  459.  
  460.  
  461. //      if (key == GLFW_KEY_1)alpha_+=dt_;
  462. //      if (key == GLFW_KEY_2)alpha_-=dt_;
  463. //      if (key == GLFW_KEY_3)beta_+=dt_;
  464. //      if (key == GLFW_KEY_4)beta_-=dt_;
  465.     if (key == GLFW_KEY_5)gamma_+=dt_;
  466.     if (key == GLFW_KEY_6)gamma_-=dt_;
  467.     if (key == GLFW_KEY_7)delta_+=dt_;
  468.     if (key == GLFW_KEY_8)delta_-=dt_;
  469.     if (key == GLFW_KEY_9)epsilon_+=dt_;
  470.     if (key == GLFW_KEY_0)epsilon_-=dt_;
  471.     if (key == GLFW_KEY_B)zetta_+=0.10;
  472.     if (key == GLFW_KEY_V)zetta_-=0.10;
  473.     if (key == GLFW_KEY_F)bla_+=0.1;
  474.     if (key == GLFW_KEY_G)bla_-=0.1;
  475.  
  476.     if (key == GLFW_KEY_R){
  477.         alpha_=0;
  478.         beta_ =0;
  479.         gamma_=0;
  480.         delta_=0;
  481.         epsilon_=0;
  482.         zetta_=0;
  483.         angleXAxis_=0;
  484.         angleYAxis_=-90;
  485.         angleZAxis_=0;
  486.         dt_=1;
  487.         alpha_=0;
  488.         a[0]=15;
  489.         a[1]=15;
  490.     }
  491.     if(key == GLFW_KEY_ESCAPE)exit(0);
  492. }
  493.  
  494.  
  495. int main() {
  496.   GLFWwindow* window = NULL;
  497.  
  498.   printf("Here we go!\n");
  499.  
  500.   if(!glfwInit()){
  501.     return -1;
  502.   }
  503.  
  504.   window = glfwCreateWindow(window_width_, window_height_,
  505.                             "Simple 3D Animation", NULL, NULL);
  506.   if(!window) {
  507.     glfwTerminate();
  508.     return -1;
  509.   }
  510.  
  511.   glfwMakeContextCurrent(window);
  512.  
  513.   while(!glfwWindowShouldClose(window)) {
  514.     // switch on lighting (or you don't see anything)
  515.     InitLighting();
  516.  
  517.     // set background color
  518.     glClearColor(0.8, 0.8, 0.8, 1.0);
  519.     glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
  520.     //KeyFunctions
  521.     glfwSetMouseButtonCallback(window, mouse_button_callback);
  522.     glfwSetKeyCallback(window, key_callback);
  523.     // draw the scene
  524.     Preview();
  525.  
  526.     // make it appear (before this, it's hidden in the rear buffer)
  527.     glfwSwapBuffers(window);
  528.  
  529.     glfwPollEvents();
  530.   }
  531.  
  532.   glfwTerminate();
  533.  
  534.   printf("Goodbye!\n");
  535.  
  536.   return 0;
  537. }
Advertisement
Add Comment
Please, Sign In to add comment
Public Pastes
We use cookies for various purposes including analytics. By continuing to use Pastebin, you agree to our use of cookies as described in the Cookies Policy.  OK, I Understand
Not a member of Pastebin yet?
Sign Up, it unlocks many cool features!