pqp
 All Classes
BVTQ.h
1 /*************************************************************************\
2 
3  Copyright 1999 The University of North Carolina at Chapel Hill.
4  All Rights Reserved.
5 
6  Permission to use, copy, modify and distribute this software and its
7  documentation for educational, research and non-profit purposes, without
8  fee, and without a written agreement is hereby granted, provided that the
9  above copyright notice and the following three paragraphs appear in all
10  copies.
11 
12  IN NO EVENT SHALL THE UNIVERSITY OF NORTH CAROLINA AT CHAPEL HILL BE
13  LIABLE TO ANY PARTY FOR DIRECT, INDIRECT, SPECIAL, INCIDENTAL, OR
14  CONSEQUENTIAL DAMAGES, INCLUDING LOST PROFITS, ARISING OUT OF THE
15  USE OF THIS SOFTWARE AND ITS DOCUMENTATION, EVEN IF THE UNIVERSITY
16  OF NORTH CAROLINA HAVE BEEN ADVISED OF THE POSSIBILITY OF SUCH
17  DAMAGES.
18 
19  THE UNIVERSITY OF NORTH CAROLINA SPECIFICALLY DISCLAIM ANY
20  WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF
21  MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE SOFTWARE
22  PROVIDED HEREUNDER IS ON AN "AS IS" BASIS, AND THE UNIVERSITY OF
23  NORTH CAROLINA HAS NO OBLIGATIONS TO PROVIDE MAINTENANCE, SUPPORT,
24  UPDATES, ENHANCEMENTS, OR MODIFICATIONS.
25 
26  The authors may be contacted via:
27 
28  US Mail: E. Larsen
29  Department of Computer Science
30  Sitterson Hall, CB #3175
31  University of N. Carolina
32  Chapel Hill, NC 27599-3175
33 
34  Phone: (919)962-1749
35 
36  EMail: geom@cs.unc.edu
37 
38 
39 \**************************************************************************/
40 
41 #ifndef PQP_BVTQ_H
42 #define PQP_BVTQ_H
43 
44 #include <stdio.h>
45 #include <stdlib.h>
46 #include "PQP_Compile.h"
47 
48 inline
49 int
50 LChild(int p)
51 {
52  return (2*p + 1);
53 }
54 
55 inline
56 int
57 Parent(int c)
58 {
59  return ((c - 1)/2);
60 }
61 
62 struct BVT
63 {
64  PQP_REAL d; // distance between the bvs
65  int b1, b2; // bv numbers - b1 is from model 1, b2 from model 2
66  PQP_REAL R[3][3]; // the relative rotation from b1 to b2
67  PQP_REAL T[3]; // the relative translation from b1 to b2
68  int pindex; // the index of the pointer that points to this -
69  // needed when filling the hole left by an ExtractMin
70 };
71 
72 class BVTQ
73 {
74  int size; // max number of bv tests
75  int numtests; // number of bv tests in queue
76  BVT *bvt; // an array of bv tests - seems faster than 'new' for each
77  BVT **bvtp; // the queue: an array of pointers to elts of bvt
78 
79 public:
80  BVTQ(int sz)
81  {
82  size = sz;
83  bvt = new BVT[size];
84  bvtp = new BVT*[size];
85  numtests = 0;
86  }
87  ~BVTQ() { delete [] bvt; delete [] bvtp; }
88  int Empty() { return (numtests == 0); }
89  int GetNumTests() { return numtests; }
90  int GetSize() { return size; }
91  PQP_REAL MinTest() { return bvtp[0]->d; }
92  BVT ExtractMinTest();
93  void AddTest(BVT &);
94 };
95 
96 inline
97 void
98 BVTQ::AddTest(BVT &t)
99 {
100  bvtp[numtests] = &bvt[numtests];
101 
102  *bvtp[numtests] = t;
103  bvtp[numtests]->pindex = numtests;
104 
105  BVT *temp;
106  int c = numtests;
107  int p;
108 
109  while ((c != 0) && (bvtp[(p = Parent(c))]->d >= bvtp[c]->d))
110  {
111  // swap p and c pointers
112 
113  temp = bvtp[p];
114  bvtp[p] = bvtp[c];
115  bvtp[c] = temp;
116 
117  // the bv tests pointed to by p and c need new indices
118 
119  bvtp[p]->pindex = p;
120  bvtp[c]->pindex = c;
121 
122  c = p;
123  }
124  numtests++;
125 }
126 
127 inline
128 BVT
129 BVTQ::ExtractMinTest()
130 {
131  // store min test to be extracted
132 
133  BVT min_test = *bvtp[0];
134 
135  // copy last bvt to the empty space;
136  // reset the pointer to this moved bvt
137 
138  *bvtp[0] = bvt[numtests-1];
139  bvtp[bvt[numtests-1].pindex] = bvtp[0];
140 
141  // copy the last pointer to the first
142 
143  bvtp[0] = bvtp[numtests-1];
144 
145  numtests--;
146 
147  BVT *temp;
148  int p = 0;
149  int c1,c2,c;
150 
151  while(1)
152  {
153  c1 = LChild(p);
154  c2 = c1+1;
155 
156  if (c1 < numtests)
157  {
158  if (c2 < numtests)
159  {
160  // p has both children, promote the minimum
161 
162  if (bvtp[c1]->d < bvtp[c2]->d) c = c1; else c = c2;
163 
164  if (bvtp[c]->d < bvtp[p]->d)
165  {
166  temp = bvtp[p];
167  bvtp[p] = bvtp[c];
168  bvtp[c] = temp;
169 
170  bvtp[p]->pindex = p;
171  bvtp[c]->pindex = c;
172 
173  p = c;
174  }
175  else
176  {
177  break;
178  }
179  }
180  else
181  {
182  // p has only left child
183 
184  if (bvtp[c1]->d < bvtp[p]->d)
185  {
186  temp = bvtp[p];
187  bvtp[p] = bvtp[c1];
188  bvtp[c1] = temp;
189 
190  bvtp[p]->pindex = p;
191  bvtp[c1]->pindex = c1;
192 
193  p = c1;
194  }
195  else
196  {
197  break;
198  }
199  }
200  }
201  else
202  {
203  // p has no children
204 
205  break;
206  }
207  }
208 
209  return min_test;
210 }
211 
212 #endif
213 
214