pf_kdtree.h
00001 /* 00002 * Player - One Hell of a Robot Server 00003 * Copyright (C) 2003 00004 * Andrew Howard 00005 * Brian Gerkey 00006 * 00007 * This program is free software; you can redistribute it and/or modify 00008 * it under the terms of the GNU General Public License as published by 00009 * the Free Software Foundation; either version 2 of the License, or 00010 * (at your option) any later version. 00011 * 00012 * This program is distributed in the hope that it will be useful, 00013 * but WITHOUT ANY WARRANTY; without even the implied warranty of 00014 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 00015 * GNU General Public License for more details. 00016 * 00017 * You should have received a copy of the GNU General Public License 00018 * along with this program; if not, write to the Free Software 00019 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 00020 * 00021 */ 00022 00023 00024 /************************************************************************** 00025 * Desc: KD tree functions 00026 * Author: Andrew Howard 00027 * Date: 18 Dec 2002 00028 * CVS: $Id: pf_kdtree.h 8108 2009-07-23 23:03:37Z thjc $ 00029 *************************************************************************/ 00030 00031 #ifndef PF_KDTREE_H 00032 #define PF_KDTREE_H 00033 00034 #include <playerconfig.h> 00035 00036 #ifdef INCLUDE_RTKGUI 00037 #include "rtk.h" 00038 #endif 00039 00040 00041 // Info for a node in the tree 00042 typedef struct pf_kdtree_node 00043 { 00044 // Depth in the tree 00045 int leaf, depth; 00046 00047 // Pivot dimension and value 00048 int pivot_dim; 00049 double pivot_value; 00050 00051 // The key for this node 00052 int key[3]; 00053 00054 // The value for this node 00055 double value; 00056 00057 // The cluster label (leaf nodes) 00058 int cluster; 00059 00060 // Child nodes 00061 struct pf_kdtree_node *children[2]; 00062 00063 } pf_kdtree_node_t; 00064 00065 00066 // A kd tree 00067 typedef struct 00068 { 00069 // Cell size 00070 double size[3]; 00071 00072 // The root node of the tree 00073 pf_kdtree_node_t *root; 00074 00075 // The number of nodes in the tree 00076 int node_count, node_max_count; 00077 pf_kdtree_node_t *nodes; 00078 00079 // The number of leaf nodes in the tree 00080 int leaf_count; 00081 00082 } pf_kdtree_t; 00083 00084 00085 // Create a tree 00086 extern pf_kdtree_t *pf_kdtree_alloc(int max_size); 00087 00088 // Destroy a tree 00089 extern void pf_kdtree_free(pf_kdtree_t *self); 00090 00091 // Clear all entries from the tree 00092 extern void pf_kdtree_clear(pf_kdtree_t *self); 00093 00094 // Insert a pose into the tree 00095 extern void pf_kdtree_insert(pf_kdtree_t *self, pf_vector_t pose, double value); 00096 00097 // Cluster the leaves in the tree 00098 extern void pf_kdtree_cluster(pf_kdtree_t *self); 00099 00100 // Determine the probability estimate for the given pose 00101 extern double pf_kdtree_get_prob(pf_kdtree_t *self, pf_vector_t pose); 00102 00103 // Determine the cluster label for the given pose 00104 extern int pf_kdtree_get_cluster(pf_kdtree_t *self, pf_vector_t pose); 00105 00106 00107 #ifdef INCLUDE_RTKGUI 00108 00109 // Draw the tree 00110 extern void pf_kdtree_draw(pf_kdtree_t *self, rtk_fig_t *fig); 00111 00112 #endif 00113 00114 #endif