开通博客赚积分
发布资源赚积分
分类

源码开发语言/平台
当前位置：首页 > 源码/资料 > 图形图像 > 3D图形编程 > 查看源码
Transform.cpp
资源名称：3dSimplifier.zip　[点击查看]
上传用户：lin85885
上传日期：2013-04-27
资源大小：796k
文件大小：12k
源码类别：

3D图形编程

开发平台：

Visual C++

Transform.cpp：源码内容
  1. /*________________________________________________
  2.  |                                          
  3.  |  跟踪球算法
  4.  |________________________________________________*/
  6. #include "stdafx.h"
  7. #include "Transform.h"
  8. #include <math.h>
  9. #include <GL/gl.h>
  10. #include <GL/glu.h>
  13. //---------------- vector & matrix operation -----------------------
  15. // V = ( x, y, z )
  16. void VectorCopy(VECTOR & V, float x, float y, float z)
  17. {
  18.    V.x = x;
  19.    V.y = y;
  20.    V.z = z;
  21. }
  23. // V = A 
  24. void VectorCopy(VECTOR & V, VECTOR A)
  25. {
  26.    V.x = A.x;
  27.    V.y = A.y;
  28.    V.z = A.z;
  29. }
  31. // V = A + B
  32. void VectorAdd(VECTOR & V, VECTOR A, VECTOR B)
  33. {
  34.   V.x = A.x + B.x;
  35.   V.y = A.y + B.y;
  36.   V.z = A.z + B.z;
  37. }
  39. // V = A - B
  40. void VectorSub(VECTOR & V, VECTOR A, VECTOR B)
  41. {
  42.   V.x = A.x - B.x;
  43.   V.y = A.y - B.y;
  44.   V.z = A.z - B.z;
  45. }
  47. // V = s * V
  48. void VectorScale(VECTOR & V, float s)
  49. {
  50.    V.x = s * V.x;
  51.    V.y = s * V.y;
  52.    V.z = s * V.z;
  53. }
  55. // V = A x B
  56. void VectorCross(VECTOR & V, VECTOR A, VECTOR B)
  57. {
  58.   V.x = A.y*B.z - A.z*B.y;
  59.   V.y = A.z*B.x - A.x*B.z;
  60.   V.z = A.x*B.y - A.y*B.x;
  61. }
  63. // dot = A . B
  64. void VectorDot(float & dot, VECTOR A, VECTOR B)
  65. {
  66.   dot = A.x*B.x + A.y*B.y + A.z*B.z;
  67. }
  69. // len = |V|
  70. void VectorLength(float & len, VECTOR V)
  71. {
  72.    len = sqrt(V.x*V.x + V.y*V.y + V.z*V.z);
  73. }
  75. // V = V/(|V|)
  76. void VectorNormalize(VECTOR & V)
  77. {
  78.   float len;
  80.   len = sqrt(V.x*V.x + V.y*V.y + V.z*V.z);
  82.   if(len<=0.0000001f)
  83.     {
  84.   V.x = 0.0f;
  85.   V.y = 0.0f;
  86.   V.z = 0.0f;
  87.     }
  88.   else
  89.     {
  90.   V.x /= len;
  91.   V.y /= len;
  92.   V.z /= len;
  93.     }
  94. }
  96. // M = M1 * M2
  97. void MultMatrixf( float *M,  float *M1, float *M2,
  98.  int m1Row, int m1Col, int m2Row, int m2Col)
  99. {
  100.   if ( m1Col != m2Row )  return ;
  101.   int size = m1Row * m2Col;
  102.   
  103.   for( int i=0, i1=0; i<size; i+= m2Col, i1+= m1Col)
  104.     for( int j = 0; j < m2Col; j++)
  105.     {
  106.       M[i+j] = 0.0f;
  107.       for( int k=0, k1=0; k<m1Col; k++, k1+=m2Col)
  108.   M[i+j] = M[i+j] + M1[i1+k] * M2[k1+j];
  109.     }                         
  110. }
  112. //------------------  inverse  -------------------------
  113. static int LUDcmp( float *data, int *index, int n)
  114. {
  115.   const float TINY = 1.0e-20f;
  116.   float *vv= new float [n];
  117.   float temp, big;
  118.   int i, ii, j, jj, k, kk, imax = 0;
  120.   for (i = 0, ii= 0; i < n; i++, ii += n)
  121.   {
  122.     big = 0.0f;
  123.     for(j = 0; j < n; j++)
  124.      if((temp = fabs( data[ii+j])) > big)  big = temp;
  125.        
  126. if( big == 0.0f )
  127.     {
  128.       delete vv;
  129.   return 0;
  130.     }
  131.      
  132. vv[i] = 1.0f /big;
  133.   }
  134.   for( j = 0, jj = 0; j < n; j++, jj += n)
  135.   {
  136.      for( i = 0, ii = 0; i <= j; i++, ii += n)
  137.      {
  138.        temp = data[ii+j];
  139.        for( int k = 0, kk = 0; k < i; k++, kk += n)
  140.        temp -= data[ii+k] * data[kk+j];
  141.        data[ii+j] = temp;
  142.      }
  143.      big = 0.0f;
  144.      imax = j;
  145.      for ( i = j+1, ii = i * n; i < n; i++, ii += n)
  146.      {
  147.    temp = data[ii+j];
  148.    for( k = 0, kk = 0; k < j; k++, kk += n)
  149.       temp -= data[ii+k] * data[kk+j];
  150.    data[ii+j] = temp;
  151.    if(( temp = vv[i] * fabs(temp)) > big)
  152.    {
  153.      big = temp;
  154.      imax = i;
  155.         }
  156.      }
  157.      if( j != imax)
  158.      {
  159.    ii = imax * n;
  160.    for ( k = 0; k < n; k++)
  161.    {
  162.    temp = data[ii+k];
  163.    data[ii+k] = data[jj+k];
  164.    data[jj+k] = temp;
  165.    }
  166.    vv[imax] = vv[j];
  167.      }
  168.      index[j] = imax;
  169.      if( data[jj+j] == 0.0f ) data[jj+j] = TINY;
  170.      if( j < n)
  171.      {
  172.    temp = 1.0f/data[jj+j];
  173.    for ( i = j+1, ii = i * n; i < n; i++, ii += n)
  174.    data[ii+j] *= temp;
  175.      }
  176.    }
  177.    delete vv;
  178.    return 1;
  179. }
  181. void LUBksb( float *data, int *index, float *b, int n)
  182. {
  183.   int ii, i, j, k = -1, ip;
  184.   float sum;
  185.   for ( i = 0, ii = 0; i < n; i++, ii += n)
  186.   {
  187.     ip = index[i];
  188.     sum = b[ip];
  189.     b[ip] = b[i];
  190.     if ( k != -1)
  191.       for ( j = k; j < i; j++)    sum -= data[ii+j]*b[j];
  192.     else if ( sum)
  193.       k = i;
  194.     b[i] = sum;
  195.   }
  196.   for( i = n-1, ii = i * n; i >=0; i--, ii -= n)
  197.   {
  198.     sum = b[i];
  199.     for( j = i + 1; j < n; j++)   sum -= data[ii+j] * b[j];
  200.     b[i] = sum / data[ii+i];
  201.   }
  202. }
  204. int InvMatrixf(float *data, int n)
  205. {
  206.   int *index = new int [n];
  207.   if ( LUDcmp( data, index, n) == 0)
  208.   {
  209.      delete index;
  210.      return 0;
  211.   }
  212.   float *b = new float [n];
  213.   float *inva = new float [n*n];
  214.   for ( int i = 0; i < n; i++)
  215.   {
  216.     for( int j = 0; j < n; j++)   b[j] = 0.0f;
  218.     b[i] = 1.0f;
  219.     LUBksb( data, index, b, n);
  220.     int jj;
  221.     for ( j = 0, jj = 0; j < n; j++, jj += n)   inva[jj+i] = b[j];
  222.   }
  223.   for ( i = 0; i < n*n; i++)   data[i] = inva[i];
  224.   delete b;
  225.   delete inva;
  226.   delete index;
  227.   return 1;
  228. }
  230. // transpose matrix
  231. void TransMatrix( float *M, int m, int n)
  232. {
  233.    float *temp = new float [m*n];
  234.    for( int i = 0, ii = 0; i < m; i++, ii += m)
  235.    {
  236.      for( int j = 0, jj = 0; j < n; j++, jj += n)
  237.         temp[ii+j] = M[jj+i];
  238.    }
  240.    for( i = 0; i < m*n; i++)  M[i] = temp[i];
  242.    delete temp;
  243.    return;
  244. }
  247. //-------------------------------------------------------
  248. // 跟踪球算法实现
  249. //-------------------------------------------------------
  250. // 参考 《科学计算可视化算法与系统》 P232 - P233
  251. //-------------------------------------------------------
  252. // 周昆，1998，3，23
  254. void Trackball(int   event,   float radius ,
  255.   float centerX, float centerY, float mouseX, float mouseY,
  256.   float &axisX , float &axisY , float &axisZ, float &angle )
  257. {
  259. //------ TB trackball ----
  260. static TrackballStruct TB;
  261.   
  262. if(event==0)  // TB_START
  263. {
  264. TB.centerX = centerX; 
  265. TB.centerY = centerY; 
  266. TB.radius  = radius ;
  267. TB.pointX1 = mouseX ;   // !
  268. TB.pointY1 = mouseY ;   // !
  269. return;
  270. }
  271. if(event>0)  // TB_MOVE , TB_END
  272. {
  273. float sinTao, cosTao, sinOmiga, cosOmiga, sinSita, cosSita;
  275. TB.centerX = centerX; 
  276. TB.centerY = centerY; 
  277. TB.radius  = radius ;
  278. TB.pointX2 = mouseX ;   // !
  279. TB.pointY2 = mouseY ;   // !
  281. VECTOR  V1, V2, V3; 
  282. float len1, len2, len3;
  283. float dot;
  284. float dx,dy;
  286. // 计算 sinTao, cosTao
  287. dx = TB.pointX2 - TB.pointX1;
  288. dy = TB.pointY2 - TB.pointY1;
  289. VectorCopy(V1, dx, dy, 0.0f);
  290.   
  291. dx = TB.pointX1 - TB.centerX;
  292. dy = TB.pointY1 - TB.centerY;
  293. VectorCopy(V2, dx, dy, 0.0f);
  295. VectorCross(V3, V2, V1);
  296. VectorLength(len1, V1);
  297. VectorLength(len2, V2);
  298. VectorLength(len3, V3);
  299.   
  300. if( len1<=0.00001f  || len2<=0.00001f )
  301. { 
  302. sinTao = 0.0f;
  303. cosTao = 1.0f;
  304. } 
  305. else
  306. {
  307. if( V3.z>=0.0f ) sinTao =  len3/(len1*len2);
  308. else             sinTao = -len3/(len1*len2); 
  309. VectorDot(dot, V1, V2);
  310. cosTao = dot/(len1*len2);
  311. }
  313. // 计算sinOmiga,cosOmiga
  314. sinOmiga  = sqrt(dx*dx + dy*dy);
  315. sinOmiga  = sinOmiga/TB.radius;
  316. if(sinOmiga>1.0f)
  317. sinOmiga=1.0;
  318. //99/12/28
  319. sinOmiga = 0.0f;
  320. cosOmiga=sqrt(1.0-sinOmiga*sinOmiga);
  322. //计算sinSita,cosSita
  323. len1 = sqrt(dx*dx + dy*dy);
  324. if(len1<=0.00001f)
  325. {  
  326. sinSita = 0.0f;
  327. cosSita = 1.0f;
  328. }
  329. else
  330. {
  331. sinSita = dy/len1;
  332. cosSita = dx/len1;
  333. }
  335. // 计算旋转角度，用《角度》表示
  336. dx = TB.pointX2 - TB.pointX1;
  337. dy = TB.pointY2 - TB.pointY1;
  338. len1= sqrt(dx*dx + dy*dy);
  339. angle = (len1/TB.radius)*45.0f;
  342. TB.pointX1 = TB.pointX2;
  343. TB.pointY1 = TB.pointY2;
  345. float M [1][3] = { 0.0f,     0.0f,    0.0f      };
  346. float M0[1][3] = { 0.0f,     0.0f,    0.0f      };
  347. float M1[1][3] = {-sinTao,   cosTao,  0.0f      };
  348. float M2[3][3] = { cosOmiga, 0.0f,   -sinOmiga,
  349. 0.0f,     1.0f,   0.0f,
  350. sinOmiga, 0.0f,   cosOmiga  };
  351. float M3[3][3] = { cosSita,  sinSita, 0.0f,
  352. -sinSita,  cosSita, 0.0f,
  353. 0.0f,     0.0f,    1.0f      };
  355. MultMatrixf( &M0[0][0], &M1[0][0], &M2[0][0], 1, 3, 3, 3);
  356. MultMatrixf( &M [0][0], &M0[0][0], &M3[0][0], 1, 3, 3, 3);
  358. //----- output -----------
  359. axisX = M[0][0];
  360. axisY = M[0][1];
  361. axisZ = M[0][2];
  362. } // end of TB_MOVE 
  363. }
  365. // transform point P(x,y,z) from object coordinate to world coordinate
  366. void ObjectToWorld(float point[3], float result[3])
  367. {
  368. // (1) get current modelview matrix
  369. float temp[4][4];
  370. glGetFloatv(GL_MODELVIEW_MATRIX, &temp[0][0]);
  372. // (2) transpose the modelview matrix
  373. TransMatrix( &temp[0][0], 4, 4);
  375. float a[4][1];
  376. float b[4][1];
  377. a[0][0]=point[0];
  378. a[1][0]=point[1];
  379. a[2][0]=point[2];
  380. a[3][0]=1.0f ;
  381.     
  382. // (3) transform (x,y,z) from object coordinate to world coordinate    
  383.     MultMatrixf( &b[0][0], &temp[0][0], &a[0][0], 4, 4, 4, 1);
  384.     result[0]=b[0][0];
  385.     result[1]=b[1][0];
  386.     result[2]=b[2][0];
  387. }
  389. // transform point P(x,y,z) from world coordinate to object coordinate
  390. void WorldToObject(float point[3], float result[3])
  391. {
  392.   // (1) get current modelview matrix
  393.   float temp[4][4];
  394.   glGetFloatv(GL_MODELVIEW_MATRIX, &temp[0][0]);
  396.   // (2) transpose the modelview matrix
  397.   TransMatrix( &temp[0][0], 4, 4);
  399.   // (3) inverse the modelview matrix
  400.   if( InvMatrixf(&temp[0][0], 4) )
  401.   {
  402. float a[4][1];
  403. float b[4][1];
  404. a[0][0]=point[0];
  405. a[1][0]=point[1];
  406. a[2][0]=point[2];
  407. a[3][0]=1.0f ;
  408.     
  409. // (4) transform (x,y,z) from world coordinate to object coordinate    
  410.     MultMatrixf( &b[0][0], &temp[0][0], &a[0][0], 4, 4, 4, 1);
  411.     result[0]=b[0][0];
  412.     result[1]=b[1][0];
  413.     result[2]=b[2][0];
  414.   }
  415. }
  417. // rotate in world coordinate
  418. void WorldRotate(float point[3], float normal[3], float angle)
  419. {
  420. float pp[3];
  421. float nn[3];
  422.     float tt[3];
  424. float t[3]={0.0f,0.0f,0.0f};
  425. WorldToObject(point, pp);
  426. WorldToObject(normal, nn);
  427. WorldToObject(t, tt);
  428. nn[0]-=tt[0];
  429. nn[1]-=tt[1];
  430. nn[2]-=tt[2];
  431. glTranslatef(pp[0],pp[1],pp[2]);
  432. glRotatef(angle,nn[0],nn[1],nn[2]);
  433. glTranslatef(-pp[0],-pp[1],-pp[2]);
  434. }
  436. // translate in world coordinate
  437. void WorldTranslate(float p[3])
  438. {
  439.   float pp[3];
  440.   float tt[3];
  442.   float t[3]={0.0f,0.0f,0.0f};
  443.   WorldToObject(p, pp);
  444.   WorldToObject(t, tt);
  445.   pp[0]-=tt[0];
  446.   pp[1]-=tt[1];
  447.   pp[2]-=tt[2];
  448.   glTranslatef(pp[0],pp[1],pp[2]);
  449. }
  451. // calculate eye axes vectors
  452. void SetEyeStruct(float eyeX,    float eyeY,    float eyeZ, 
  453.  float centerX, float centerY, float centerZ, 
  454.  float upX,     float upY,     float upZ,
  455.  EYE  & eye)
  456. {
  457.   eye.origin.x=eyeX;
  458.   eye.origin.y=eyeY;
  459.   eye.origin.z=eyeZ;
  460.   eye.center.x=centerX;
  461.   eye.center.y=centerY;
  462.   eye.center.z=centerZ;
  463.   eye.up.x=upX;
  464.   eye.up.y=upY;
  465.   eye.up.z=upZ;
  467.   // 计算眼睛各坐标轴的矢量
  468.   VECTOR v1,v2,v3;
  469.   VectorSub(v1,eye.center,eye.origin);
  470.   VectorNormalize(v1);
  471.   VectorCopy(v2,eye.up);
  472.   VectorCross(v3,v2,v1);
  473.   VectorNormalize(v3);
  474.   VectorCross(v2,v3,v1);
  475.   VectorCopy(eye.axisX,v3);
  476.   VectorCopy(eye.axisY,v2);
  477.   VectorCopy(eye.axisZ,v1);
  478. }
  480. // transform point P(x,y,z) from world coordinate to eye coordinate
  481. void WorldToEye(float point[3], float result[3], EYE eye)
  482. {
  483. float tmp[3][3];
  484. float a[3][1];
  485. float b[3][1];
  487. tmp[0][0]=(eye.axisX).x;
  488. tmp[1][0]=(eye.axisX).y;
  489. tmp[2][0]=(eye.axisX).z;
  490. tmp[0][1]=(eye.axisY).x;
  491. tmp[1][1]=(eye.axisY).y;
  492. tmp[2][1]=(eye.axisY).z;
  493. tmp[0][2]=(eye.axisZ).x;
  494. tmp[1][2]=(eye.axisZ).y;
  495. tmp[2][2]=(eye.axisZ).z;
  497. InvMatrixf(&tmp[0][0],3);
  498. a[0][0]=point[0];
  499. a[1][0]=point[1];
  500. a[2][0]=point[2];
  502. MultMatrixf( &b[0][0], &tmp[0][0], &a[0][0], 3, 3, 3, 1);
  503. result[0]=b[0][0]-eye.origin.x;
  504. result[1]=b[1][0]-eye.origin.y;
  505. result[2]=b[2][0]+eye.origin.z;
  506. }
  508. // transform point P(x,y,z) from eye coordinate to world coordinate
  509. void EyeToWorld(float point[3], float result[3], EYE eye)
  510. {
  511.   VECTOR v1,v2,v3,v4,v5,v6;
  513.   VectorCopy(v1,eye.axisZ);
  514.   VectorCopy(v2,eye.axisY);
  515.   VectorCopy(v3,eye.axisX);
  516.   VectorScale(v3,point[0]);
  517.   VectorScale(v2,point[1]);
  518.   VectorScale(v1,point[2]);
  519.   VectorAdd(v4,v1,v2);
  520.   VectorAdd(v5,v4,v3);
  521.   VectorAdd(v6,eye.origin,v5);
  522.   result[0]=v6.x;
  523.   result[1]=v6.y;
  524.   result[2]=v6.z;
  525. }
  527. // rotate in eye coordinate
  528. void EyeRotate(float point[3], float normal[3], float angle, 
  529.   EYE eye)
  530. {
  531.   float pp[3];
  532.   float nn[3];
  533.   float tt[3];
  535.   float t[3]={0.0f,0.0f,0.0f};
  536.   EyeToWorld(point, pp, eye);
  537.   EyeToWorld(normal, nn, eye);
  538.   EyeToWorld(t, tt, eye);
  539.   nn[0]-=tt[0];
  540.   nn[1]-=tt[1];
  541.   nn[2]-=tt[2];
  542.   WorldRotate(pp, nn, angle);
  543. }
  545. // translate in eye coordinate
  546. void EyeTranslate(float p[3], EYE eye)
  547. {
  548.   float pp[3];
  549.   float tt[3];
  551.   float t[3]={0.0f,0.0f,0.0f};
  552.   EyeToWorld(p, pp, eye);
  553.   EyeToWorld(t, tt, eye);
  554.   pp[0]-=tt[0];
  555.   pp[1]-=tt[1];
  556.   pp[2]-=tt[2];
  557.   WorldTranslate(pp);
  558. }