RomanNum3ral
/
open_toontown_panda3d
mirror of https://github.com/open-toontown/panda3d.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity
open_toontown_panda3d/panda/src/grutil/arToolKit.cxx

441 lines
14 KiB
C++
Raw Blame History

// Filename: arToolKit.cxx
// Created by: jyelon (01Nov2007)
//
////////////////////////////////////////////////////////////////////
//
// PANDA 3D SOFTWARE
// Copyright (c) Carnegie Mellon University. All rights reserved.
//
// All use of this software is subject to the terms of the revised BSD
// license. You should have received a copy of this license along
// with this source code in a file named "LICENSE."
//
////////////////////////////////////////////////////////////////////
#include "arToolKit.h"
#ifdef HAVE_ARTOOLKIT
#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"
};
ARToolKit::PatternTable ARToolKit::_pattern_table;
static void change_size( ARParam *source, int xsize, int ysize, ARParam *newparam )
{
int i;
newparam->xsize = xsize;
newparam->ysize = ysize;
double xscale = (double)xsize / (double)(source->xsize);
double yscale = (double)ysize / (double)(source->ysize);
for( i = 0; i < 4; i++ ) {
newparam->mat[0][i] = source->mat[0][i] * xscale;
newparam->mat[1][i] = source->mat[1][i] * yscale;
newparam->mat[2][i] = source->mat[2][i];
}
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::make
// Access: Private
// 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, double marker_size) {
if (AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGRA &&
AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_RGBA &&
AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_RGB &&
AR_DEFAULT_PIXEL_FORMAT != AR_PIXEL_FORMAT_BGR) {
grutil_cat.error() <<
"The copy of ARToolKit that you are using is not compiled "
"for RGB, BGR, RGBA or ARGB input. Panda3D cannot use "
"this copy of ARToolKit. Please modify the ARToolKit's "
"config file and compile it again.\n";
return 0;
}
if (camera.is_empty()) {
grutil_cat.error() << "ARToolKit: invalid camera nodepath\n";
return 0;
}
PandaNode *node = camera.node();
if ((node == 0) || (node->get_type() != Camera::get_class_type())) {
grutil_cat.error() << "ARToolKit: invalid camera nodepath\n";
return 0;
}
Camera *cam = DCAST(Camera, node);
Lens *lens = cam->get_lens();
if (lens->get_type() != PerspectiveLens::get_class_type()) {
grutil_cat.error() << "ARToolKit: supplied camera node must be perspective.\n";
return 0;
}
ARParam wparam;
string fn = paramfile.to_os_specific();
if( arParamLoad(fn.c_str(), 1, &wparam) < 0 ) {
grutil_cat.error() << "Cannot load ARToolKit camera config\n";
return 0;
}
arParamDisp(&wparam);
double xfov, yfov;
analyze_fov(wparam.mat, 640, 480, xfov, yfov);
lens->set_fov(xfov, yfov);
ARToolKit *result = new ARToolKit();
result->_camera = camera;
result->_camera_param = new ARParam;
result->_threshold = 0.5;
result->_marker_size = marker_size;
result->_have_prev_conv = false;
memcpy(result->_camera_param, &wparam, sizeof(wparam));
return result;
}
////////////////////////////////////////////////////////////////////
// Function: ARToolKit::cleanup
// Access: private
// Description: Pre-destructor deallocation and cleanup.
////////////////////////////////////////////////////////////////////
void ARToolKit::
cleanup() {
if (_camera_param) {
ARParam *param = (ARParam *)_camera_param;
delete param;
_camera_param = 0;
}
}
////////////////////////////////////////////////////////////////////
// Function: ARToolKit::Constructor
// Access: Private
// Description: Use ARToolKit::make to create an ARToolKit.
////////////////////////////////////////////////////////////////////
ARToolKit::
ARToolKit() {
}
////////////////////////////////////////////////////////////////////
// Function: ARToolKit::Destructor
// Access: Published
// Description:
////////////////////////////////////////////////////////////////////
ARToolKit::
~ARToolKit() {
cleanup();
}
////////////////////////////////////////////////////////////////////
// Function: ARToolKit::get_pattern
// Access: Private
// Description: Load the specified pattern into the toolkit, and
// return the pattern index. Initially, the pattern
// is inactive.
////////////////////////////////////////////////////////////////////
int ARToolKit::
get_pattern(const Filename &filename) {
PatternTable::iterator ptf = _pattern_table.find(filename);
if (ptf != _pattern_table.end()) {
return (*ptf).second;
}
string fn = filename.to_os_specific();
int id = arLoadPatt(fn.c_str());
if (id < 0) {
grutil_cat.error() << "Could not load AR ToolKit Pattern: " << fn << "\n";
return -1;
}
arDeactivatePatt(id);
_pattern_table[filename] = id;
return id;
}
////////////////////////////////////////////////////////////////////
// Function: ARToolKit::attach_pattern
// Access: Public
// Description: Associates the specified glyph with the specified
// NodePath. Each time you call analyze, ARToolKit
// will update the NodePath's transform. If the node
// is not visible, its scale will be set to zero.
////////////////////////////////////////////////////////////////////
void ARToolKit::
attach_pattern(const Filename &filename, NodePath path) {
int patt = get_pattern(filename);
if (patt < 0) return;
_controls[patt] = path;
}
////////////////////////////////////////////////////////////////////
// Function: ARToolKit::detach_patterns
// Access: Public
// Description: Dissociates all patterns from all NodePaths.
////////////////////////////////////////////////////////////////////
void ARToolKit::
detach_patterns() {
_controls.clear();
}
////////////////////////////////////////////////////////////////////
// Function: ARToolKit::analyze
// Access: Public
// Description: Analyzes the non-pad region of the specified texture.
// This causes all attached nodepaths to move.
// The parameter do_flip_texture is true by default,
// because Panda's representation of textures is
// upside down from ARToolKit. If you already have
// a texture that's upside-down, however, you should
// set it to false.
////////////////////////////////////////////////////////////////////
void ARToolKit::
analyze(Texture *tex, bool do_flip_texture) {
// We shouldn't assert on has_ram_image since it also returns false
// when there is a ram image but its not updated for this frame.
//nassertv(tex->has_ram_image());
nassertv(tex->get_ram_image_compression() == Texture::CM_off);
nassertv(tex->get_component_type() == Texture::T_unsigned_byte);
nassertv(tex->get_texture_type() == Texture::TT_2d_texture);
if (tex->get_num_components() != 3 && tex->get_num_components() != 4) {
grutil_cat.error() << "ARToolKit can only analyze RGB and RGBA textures.\n";
return;
}
int padx = tex->get_pad_x_size();
int pady = tex->get_pad_y_size();
int xsize = tex->get_x_size() - padx;
int ysize = tex->get_y_size() - pady;
int pagesize = xsize * ysize * 4;
nassertv((xsize > 0) && (ysize > 0));
ARParam cparam;
change_size( (ARParam*)_camera_param, xsize, ysize, &cparam );
arInitCparam( &cparam );
// Pack the data into a buffer with no padding and invert the video
// vertically (panda's representation is upside down from ARToolKit)
CPTA_uchar ri = tex->get_ram_image();
const unsigned char *ram = ri.p();
unsigned char *data;
if (AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_RGB ||
AR_DEFAULT_PIXEL_FORMAT == AR_PIXEL_FORMAT_BGR) {
data = new unsigned char[xsize * ysize * 3];
int dstlen = xsize * 3;
if (tex->get_num_components() == 3) {
int srclen = (xsize + padx) * 3;
if (do_flip_texture) {
for (int y=0; y<ysize; y++) {
int invy = (ysize - y - 1);
memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
}
} else if (dstlen == srclen) {
memcpy(data, ram, ysize * srclen);
} else {
for (int y=0; y<ysize; y++) {
memcpy(data + y * dstlen, ram + y * srclen, dstlen);
}
}
} else {
// Chop off the alpha component.
int srclen = (xsize + padx) * 4;
if (do_flip_texture) {
for (int y=0; y<ysize; y++) {
int invy = (ysize - y - 1);
for (int x=0; x<xsize; x++) {
memcpy(data + invy * dstlen + x * 3, ram + y * srclen + x * 4, 3);
}
}
} else {
for (int y=0; y<ysize; y++) {
for (int x=0; x<xsize; x++) {
memcpy(data + y * dstlen + x * 3, ram + y * srclen + x * 4, 3);
}
}
}
}
} else { // ARToolKit wants RGBA.
data = new unsigned char[xsize * ysize * 4];
int dstlen = xsize * 4;
if (tex->get_num_components() == 3) {
// Until we find a better solution, we'll need to add the Alpha component ourselves.
int srclen = (xsize + padx) * 3;
if (do_flip_texture) {
for (int y=0; y<ysize; y++) {
int invy = (ysize - y - 1);
memcpy(data + invy * dstlen, ram + y * srclen, dstlen - 1);
for (int x=0; x<xsize; x++) {
data[invy * dstlen + x * 4 + 3] = -1;
}
}
} else {
for (int y=0; y<ysize; y++) {
memcpy(data + y * dstlen, ram + y * srclen, dstlen - 1);
for (int x=0; x<xsize; x++) {
data[y * dstlen + x * 4 + 3] = -1;
}
}
}
} else {
int srclen = (xsize + padx) * 4;
if (do_flip_texture) {
for (int y=0; y<ysize; y++) {
int invy = (ysize - y - 1);
memcpy(data + invy * dstlen, ram + y * srclen, dstlen);
}
} else if (dstlen == srclen) {
memcpy(data, ram, ysize * srclen);
} else {
for (int y=0; y<ysize; y++) {
memcpy(data + y * dstlen, ram + y * srclen, dstlen);
}
}
}
}
Controls::const_iterator ctrli;
for (ctrli=_controls.begin(); ctrli!=_controls.end(); ctrli++) {
arActivatePatt((*ctrli).first);
}
ARMarkerInfo *marker_info;
int marker_num;
if (arDetectMarker(data, _threshold * 256, &marker_info, &marker_num) < 0) {
grutil_cat.error() << "ARToolKit detection error.\n";
delete data;
return;
}
for (ctrli=_controls.begin(); ctrli!=_controls.end(); ctrli++) {
NodePath np = (*ctrli).second;
int pattern = (*ctrli).first;
arDeactivatePatt(pattern);
double conf = -1;
int best = -1;
for (int i=0; i<marker_num; i++) {
if (marker_info[i].id == pattern) {
if (marker_info[i].cf >= conf) {
conf = marker_info[i].cf;
best = i;
}
}
}
if (conf > 0.0) {
ARMarkerInfo *inf = &marker_info[best];
double center[2];
center[0] = 0.0;
center[1] = 0.0;
if (_have_prev_conv) {
arGetTransMatCont(inf, _prev_conv, center, _marker_size, _prev_conv);
} else {
arGetTransMat(inf, center, _marker_size, _prev_conv);
}
LMatrix4f mat;
for (int i=0; i<4; i++) {
mat(i,0) = _prev_conv[0][i];
mat(i,1) = _prev_conv[2][i];
mat(i,2) = -_prev_conv[1][i];
mat(i,3) = 0.0;
}
mat(3,3) = 1.0;
LVecBase3f scale, shear, hpr, pos;
decompose_matrix(mat, scale, shear, hpr, pos);
if (np.get_parent().is_empty()) {
grutil_cat.error() << "NodePath must have a parent.\n";
} else {
np.set_pos_hpr(_camera, pos, hpr);
}
np.show();
} else {
np.hide();
}
}
delete data;
}
#endif // HAVE_ARTOOLKIT
Reference in New Issue View Git Blame Copy Permalink