ARToolKit now basically functional

doc/makepanda/makepanda.py

@@ -2600,7 +2600,8 @@ EnqueueIgate(ipath=IPATH, opts=OPTS, outd='libmovies.in', obj='libmovies_igate.o
 IPATH=['panda/src/grutil']
 OPTS=['BUILDING_PANDA',  'FFMPEG', 'ARTOOLKIT']
 EnqueueCxx(ipath=IPATH, opts=OPTS, src='multitexReducer.cxx', obj='grutil_multitexReducer.obj')
-EnqueueCxx(ipath=IPATH, opts=OPTS, src='grutil_composite.cxx', obj='grutil_composite.obj')
+EnqueueCxx(ipath=IPATH, opts=OPTS, src='grutil_composite1.cxx', obj='grutil_composite1.obj')
+EnqueueCxx(ipath=IPATH, opts=OPTS, src='grutil_composite2.cxx', obj='grutil_composite2.obj')
 EnqueueIgate(ipath=IPATH, opts=OPTS, outd='libgrutil.in', obj='libgrutil_igate.obj',
             src='panda/src/grutil',  module='panda', library='libgrutil',
             skip=[], also=["multitexReducer.cxx","grutil_composite.cxx"])
@@ -2698,7 +2699,7 @@ OBJFILES=['panda_panda.obj', 'libpanda_module.obj',
           'pgraph_composite1.obj', 'pgraph_composite2.obj', 'pgraph_composite3.obj', 'pgraph_composite4.obj', 'libpgraph_igate.obj',
           'cull_composite.obj',
 	  'movies_composite1.obj', 'libmovies_igate.obj',
-          'grutil_multitexReducer.obj', 'grutil_composite.obj', 'libgrutil_igate.obj',
+          'grutil_multitexReducer.obj', 'grutil_composite1.obj', 'grutil_composite2.obj', 'libgrutil_igate.obj',
           'chan_composite.obj', 'libchan_igate.obj',
           'pstatclient_composite.obj', 'libpstatclient_igate.obj',
           'char_composite.obj', 'libchar_igate.obj',
@@ -2745,7 +2746,7 @@ EnqueueLink(opts=OPTS, dll='libpanda.dll', obj=OBJFILES, xdep=[
 #
 
 IPATH=['panda/src/skel']
-OPTS=['BUILDING_PANDASKEL']
+OPTS=['BUILDING_PANDASKEL', 'ARTOOLKIT']
 EnqueueCxx(ipath=IPATH, opts=OPTS, src='skel_composite.cxx', obj='skel_composite.obj')
 EnqueueIgate(ipath=IPATH, opts=OPTS, outd='libskel.in', obj='libskel_igate.obj',
              src='panda/src/skel',  module='pandaskel', library='libskel',
@@ -2755,9 +2756,10 @@ EnqueueIgate(ipath=IPATH, opts=OPTS, outd='libskel.in', obj='libskel_igate.obj',
 # DIRECTORY: panda/metalibs/panda
 #
 
+OPTS=['BUILDING_PANDASKEL', 'ARTOOLKIT', 'ADVAPI']
 EnqueueImod(ipath=IPATH, opts=OPTS, obj='libpandaskel_module.obj',
             module='pandaskel', library='libpandaskel', files=["libskel.in","libmovies.in"])
-EnqueueLink(dll='libpandaskel.dll', opts=['ADVAPI'], obj=[
+EnqueueLink(dll='libpandaskel.dll', opts=OPTS, obj=[
     'skel_composite.obj',
     'libskel_igate.obj',
     'libpandaskel_module.obj',

panda/src/grutil/Sources.pp

@@ -3,12 +3,12 @@
 
 #define USE_PACKAGES opencv ffmpeg
 
-#begin lib_target
+#begin lib_target]
   #define TARGET grutil
   #define LOCAL_LIBS \
     display text pgraph gobj linmath putil movies audio
     
-  #define COMBINED_SOURCES $[TARGET]_composite1.cxx 
+  #define COMBINED_SOURCES $[TARGET]_composite1.cxx $[TARGET]_composite2.cxx
 
   #define SOURCES \
     pipeOcclusionCullTraverser.I pipeOcclusionCullTraverser.h \
@@ -41,7 +41,6 @@
     lineSegs.cxx \
     rigidBodyCombiner.cxx
     
-
   #define INSTALL_HEADERS \
     cardMaker.I cardMaker.h \
     arToolKit.I arToolKit.h \

panda/src/grutil/arToolKit.cxx

@@ -23,6 +23,8 @@
 #include "pandaNode.h"
 #include "camera.h"
 #include "perspectiveLens.h"
+#include "lvecBase3.h"
+#include "compose_matrix.h"
 #include "config_grutil.h"
 extern "C" {
   #include "artools.h"
@@ -30,63 +32,96 @@ extern "C" {
 
 ARToolKit::PatternTable ARToolKit::_pattern_table;
 
-static void argConvGLcpara2( double cparam[3][4], int width, int height, double gnear, double gfar, double m[16] )
+static void change_size( ARParam *source, int xsize, int ysize, ARParam *newparam )
 {
-    double   icpara[3][4];
+    int     i;
-    double   trans[3][4];
-    double   p[3][3], q[4][4];
-    int      i, j;
 
-    if( arParamDecompMat(cparam, icpara, trans) < 0 ) {
+    newparam->xsize = xsize;
-        printf("gConvGLcpara: Parameter error!!\n");
+    newparam->ysize = ysize;
-        exit(0);
-    }
-
-    for( i = 0; i < 3; i++ ) {
-        for( j = 0; j < 3; j++ ) {
-            p[i][j] = icpara[i][j] / icpara[2][2];
-        }
-    }
-    q[0][0] = (2.0 * p[0][0] / width);
-    q[0][1] = (2.0 * p[0][1] / width);
-    q[0][2] = ((2.0 * p[0][2] / width)  - 1.0);
-    q[0][3] = 0.0;
-
-    q[1][0] = 0.0;
-    q[1][1] = (2.0 * p[1][1] / height);
-    q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
-    q[1][3] = 0.0;
-
-    q[2][0] = 0.0;
-    q[2][1] = 0.0;
-    q[2][2] = (gfar + gnear)/(gfar - gnear);
-    q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
-
-    q[3][0] = 0.0;
-    q[3][1] = 0.0;
-    q[3][2] = 1.0;
-    q[3][3] = 0.0;
 
+    double xscale = (double)xsize / (double)(source->xsize);
+    double yscale = (double)ysize / (double)(source->ysize);
     for( i = 0; i < 4; i++ ) {
-        for( j = 0; j < 3; j++ ) {
+      newparam->mat[0][i] = source->mat[0][i] * xscale;
-            m[i+j*4] = q[i][0] * trans[0][j]
+      newparam->mat[1][i] = source->mat[1][i] * yscale;
-                     + q[i][1] * trans[1][j]
+      newparam->mat[2][i] = source->mat[2][i];
-                     + q[i][2] * trans[2][j];
-        }
-        m[i+3*4] = q[i][0] * trans[0][3]
-                 + q[i][1] * trans[1][3]
-                 + q[i][2] * trans[2][3]
-                 + q[i][3];
     }
+
+    newparam->dist_factor[0] = source->dist_factor[0] * xscale;
+    newparam->dist_factor[1] = source->dist_factor[1] * yscale;
+    newparam->dist_factor[2] = source->dist_factor[2] / (xscale*yscale);
+    newparam->dist_factor[3] = source->dist_factor[3];
+}
+
+static void analyze_fov(double cparam[3][4], int width, int height, double &xfov, double &yfov)
+{
+  double   gnear = 10.0;
+  double gfar = 1000.0;
+  double   icpara[3][4];
+  double   trans[3][4];
+  double   p[3][3], q[4][4];
+  double   xval, yval;
+  int      i, j;
+  
+  if( arParamDecompMat(cparam, icpara, trans) < 0 ) {
+    printf("gConvGLcpara: Parameter error!!\n");
+    exit(0);
+  }
+  
+  for( i = 0; i < 3; i++ ) {
+    for( j = 0; j < 3; j++ ) {
+      p[i][j] = icpara[i][j] / icpara[2][2];
+    }
+  }
+  q[0][0] = (2.0 * p[0][0] / width);
+  q[0][1] = (2.0 * p[0][1] / width);
+  q[0][2] = ((2.0 * p[0][2] / width)  - 1.0);
+  q[0][3] = 0.0;
+  
+  q[1][0] = 0.0;
+  q[1][1] = (2.0 * p[1][1] / height);
+  q[1][2] = ((2.0 * p[1][2] / height) - 1.0);
+  q[1][3] = 0.0;
+  
+  q[2][0] = 0.0;
+  q[2][1] = 0.0;
+  q[2][2] = (gfar + gnear)/(gfar - gnear);
+  q[2][3] = -2.0 * gfar * gnear / (gfar - gnear);
+  
+  q[3][0] = 0.0;
+  q[3][1] = 0.0;
+  q[3][2] = 1.0;
+  q[3][3] = 0.0;
+  
+  xval =
+    q[0][0] * trans[0][0] + 
+    q[0][1] * trans[1][0] +
+    q[0][2] * trans[2][0];
+  yval =
+    q[1][0] * trans[0][1] + 
+    q[1][1] * trans[1][1] +
+    q[1][2] * trans[2][1];
+  
+  xfov = 2.0 * atan(1.0/xval) * (180.0/3.141592654);
+  yfov = 2.0 * atan(1.0/yval) * (180.0/3.141592654);
 }
 
 ////////////////////////////////////////////////////////////////////
-//     Function: ARToolKit::Constructor
+//     Function: ARToolKit::make
 //       Access: Private
-//  Description: 
+//  Description: Create a new ARToolKit instance.
+//               
+//               Camera must be the nodepath of a panda camera object.
+//               The panda camera's field of view is initialized to match
+//               the field of view of the physical webcam.  Each time
+//               you call analyze, all marker nodepaths will be moved
+//               into a position which is relative to this camera.
+//               The marker_size parameter indicates how large you
+//               printed the physical markers.  You should use the same
+//               size units that you wish to use in the panda code.
 ////////////////////////////////////////////////////////////////////
 ARToolKit *ARToolKit::
-make(NodePath camera, const Filename &paramfile) {
+make(NodePath camera, const Filename &paramfile, double marker_size) {
   
   if (AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGRA) {
     grutil_cat.error() <<
@@ -120,18 +155,15 @@ make(NodePath camera, const Filename &paramfile) {
   }
   
   arParamDisp(&wparam);
-  double m[16];
+  double xfov, yfov;
-  argConvGLcpara2(wparam.mat, 640, 480, 10, 1000, m);
+  analyze_fov(wparam.mat, 640, 480, xfov, yfov);
-  cerr << "Panda3D projection mat: " << lens->get_projection_mat() << "\n";
+  
-  cerr << "Artoolkit mat: "
+  lens->set_fov(xfov, yfov);
-       << m[ 0] << " " << m[ 1] << " " << m[ 2] << " " << m[ 3] << "\n"
-       << m[ 4] << " " << m[ 5] << " " << m[ 6] << " " << m[ 7] << "\n"
-       << m[ 8] << " " << m[ 9] << " " << m[10] << " " << m[11] << "\n"
-       << m[12] << " " << m[13] << " " << m[14] << " " << m[15] << "\n";
   
   ARToolKit *result = new ARToolKit();
   result->_camera = camera;
   result->_camera_param = new ARParam;
+  result->_marker_size = marker_size;
   memcpy(result->_camera_param, &wparam, sizeof(wparam));
   return result;
 }
@@ -245,7 +277,7 @@ analyze(Texture *tex, double thresh) {
   nassertv((xsize > 0) && (ysize > 0));
   
   ARParam cparam;
-  arParamChangeSize( (ARParam*)_camera_param, xsize, ysize, &cparam );
+  change_size( (ARParam*)_camera_param, xsize, ysize, &cparam );
   arInitCparam( &cparam );
 
   // Pack the data into a buffer with no padding and invert the video
@@ -276,6 +308,7 @@ analyze(Texture *tex, double thresh) {
   }
 
   for (ctrli=_controls.begin(); ctrli!=_controls.end(); ctrli++) {
+    NodePath np = (*ctrli).second;
     int pattern = (*ctrli).first;
     arDeactivatePatt(pattern);
     double conf = -1;
@@ -289,12 +322,40 @@ analyze(Texture *tex, double thresh) {
       }
     }
     if (conf > 0.0) {
-      cerr << "Marker " << pattern 
+      ARMarkerInfo *inf = &marker_info[best];
-           << " " << int(marker_info[best].vertex[0][0]) << "," << int(marker_info[best].vertex[0][1])
+      double center[2];
-           << " " << int(marker_info[best].vertex[1][0]) << "," << int(marker_info[best].vertex[1][1])
+      center[0] = 0.0;
-           << " " << int(marker_info[best].vertex[2][0]) << "," << int(marker_info[best].vertex[2][1])
+      center[1] = 0.0;
-           << " " << int(marker_info[best].vertex[3][0]) << "," << int(marker_info[best].vertex[3][1])
+      double patt_trans[3][4];
-           << "\n";
+      arGetTransMat(inf, center, _marker_size, patt_trans);
+      LMatrix4f mat;
+      for (int i=0; i<4; i++) {
+	mat(i,0) =  patt_trans[0][i];
+	mat(i,1) =  patt_trans[2][i];
+	mat(i,2) = -patt_trans[1][i];
+	mat(i,3) = 0.0;
+      }
+      mat(3,3) = 1.0;
+      LVecBase3f scale, shear, hpr, pos;
+      decompose_matrix(mat, scale, shear, hpr, pos);
+      np.set_pos(pos);
+      np.set_hpr(hpr);
+      np.show();
+      //      cerr << "Markert:\n"
+      //	   << "  Id: " << inf->id << "\n"
+      //	   << "  Area: " << inf->area << "\n"
+      //	   << "  Dir: " << inf->dir << "\n"
+      //	   << "  Cf: " << inf->cf << "\n"
+      //	   << "  Pos: " << inf->pos[0] << "," << inf->pos[1] << "\n"
+      //	   << "  Vtx0: " << inf->vertex[0][0] << "," << inf->vertex[0][1] << "\n"
+      //	   << "  Vtx1: " << inf->vertex[1][0] << "," << inf->vertex[1][1] << "\n"
+      //	   << "  Vtx2: " << inf->vertex[2][0] << "," << inf->vertex[2][1] << "\n"
+      //	   << "  Vtx3: " << inf->vertex[3][0] << "," << inf->vertex[3][1] << "\n"
+      //	   << "  Row0: " << patt_trans[0][0] << " " << patt_trans[0][1] << " " << patt_trans[0][2] << " " << patt_trans[0][3] << "\n"
+      //	   << "  Row1: " << patt_trans[1][0] << " " << patt_trans[1][1] << " " << patt_trans[1][2] << " " << patt_trans[1][3] << "\n"
+      //	   << "  Row2: " << patt_trans[2][0] << " " << patt_trans[2][1] << " " << patt_trans[2][2] << " " << patt_trans[2][3] << "\n";
+    } else {
+      np.hide();
     }
   }
   

panda/src/grutil/arToolKit.h

@@ -47,7 +47,7 @@
 class EXPCL_PANDA_GRUTIL ARToolKit {
   
 PUBLISHED:
-  static ARToolKit *make(NodePath camera, const Filename &paramfile);
+  static ARToolKit *make(NodePath camera, const Filename &paramfile, double markersize);
   ~ARToolKit();
   
   void attach_pattern(const Filename &pattern, NodePath path);
@@ -67,6 +67,8 @@ private:
   
   NodePath _camera;
   void *_camera_param;
+  
+  double _marker_size;
 };
 
 #include "arToolKit.I"

panda/src/grutil/ffmpegTexture.cxx

@@ -24,6 +24,7 @@
 #include "config_gobj.h"
 #include "config_grutil.h"
 #include "bamCacheRecord.h"
+#include "bamReader.h"
 
 TypeHandle FFMpegTexture::_type_handle;
 

panda/src/grutil/grutil_composite.cxx

@@ -1 +1,2 @@
 #include "grutil_composite1.cxx"
+#include "grutil_composite2.cxx"

panda/src/grutil/grutil_composite1.cxx

@@ -5,10 +5,3 @@
 #include "lineSegs.cxx"
 #include "fisheyeMaker.cxx"
 #include "frameRateMeter.cxx"
-#include "openCVTexture.cxx"
-#include "ffmpegTexture.cxx"
-#include "movieTexture.cxx"
-#include "nodeVertexTransform.cxx"
-#include "pipeOcclusionCullTraverser.cxx"
-#include "rigidBodyCombiner.cxx"
-

panda/src/grutil/movieTexture.cxx

@@ -25,6 +25,7 @@
 #include "config_gobj.h"
 #include "config_grutil.h"
 #include "bamCacheRecord.h"
+#include "bamReader.h"
 #include "math.h"
 #include "audioSound.h"
 

panda/src/grutil/nodeVertexTransform.h

@@ -20,6 +20,7 @@
 #define NODEVERTEXTRANSFORM_H
 
 #include "pandabase.h"
+#include "pandaNode.h"
 #include "vertexTransform.h"
 
 class FactoryParams;