Changeset 4648 in orxonox.OLD for orxonox/trunk/src/lib/collision_detection
- Timestamp:
- Jun 17, 2005, 1:31:18 AM (20 years ago)
- File:
-
- 1 edited
Legend:
- Unmodified
- Added
- Removed
-
orxonox/trunk/src/lib/collision_detection/obb_tree_node.cc
r4643 r4648 113 113 114 114 115 if(this->tmpLen1 > 2)115 if(this->tmpLen1 > 0) 116 116 { 117 117 OBBTreeNode* node1 = new OBBTreeNode(); … … 124 124 } 125 125 126 if( this->tmpLen2 > 2)126 if( this->tmpLen2 > 0) 127 127 { 128 128 OBBTreeNode* node2 = new OBBTreeNode(); … … 149 149 Vector t1, t2; //!< temporary values 150 150 float covariance[3][3]; //!< the covariance matrix 151 int mode = 3; //!< mode = 0: vertex soup, no connections, mode = 1: 3 following verteces build a triangle 151 152 152 153 this->numOfVertices = length; … … 154 155 155 156 156 157 158 /* fist compute all the convex hull face/facelets and centroids */ 159 for(int i = 0; i < length; i+=3) /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/ 160 { 161 p = verticesList[i]; 162 q = verticesList[i +1]; 163 r = verticesList[i + 2]; 164 165 t1 = p - q; t2 = p - r; 166 167 /* finding the facelet surface via cross-product */ 168 facelet[i] = 0.5f * fabs( t1.cross(t2).len() ); 169 /* update the entire convex hull surface */ 170 face += facelet[i]; 171 172 /* calculate the cetroid of the hull triangles */ 173 centroid[i] = (p + q + r) * 1/3; 174 /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */ 175 center += centroid[i] * facelet[i]; 176 } 177 /* take the average of the centroid sum */ 178 center /= face; 179 PRINTF(3)("-- Calculated Center\n"); 180 181 182 /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */ 183 for(int j = 0; j < 3; ++j) 184 { 185 for(int k = 0; k < 3; ++k) 186 { 187 for(int i = 0; i < length; i+=3) 157 if( likely(mode == 0)) 158 { 159 /* fist compute all the convex hull face/facelets and centroids */ 160 for(int i = 0; i < length; i+=3) /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/ 161 { 162 p = verticesList[i]; 163 q = verticesList[i + 1]; 164 r = verticesList[i + 2]; 165 166 t1 = p - q; t2 = p - r; 167 168 /* finding the facelet surface via cross-product */ 169 facelet[i] = 0.5f * fabs( t1.cross(t2).len() ); 170 /* update the entire convex hull surface */ 171 face += facelet[i]; 172 173 /* calculate the cetroid of the hull triangles */ 174 centroid[i] = (p + q + r) * 1/3; 175 /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */ 176 center += centroid[i] * facelet[i]; 177 } 178 /* take the average of the centroid sum */ 179 center /= face; 180 PRINTF(3)("-- Calculated Center\n"); 181 182 183 /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */ 184 for(int j = 0; j < 3; ++j) 185 { 186 for(int k = 0; k < 3; ++k) 188 187 { 189 p = verticesList[i]; 190 q = verticesList[i +1]; 191 r = verticesList[i + 2]; 192 193 covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j]* p[k] + 194 q[j] * q[k] + r[j]*r[k]) - center[j] * center[k]; 188 for(int i = 0; i < length; i+=3) 189 { 190 p = verticesList[i]; 191 q = verticesList[i + 1]; 192 r = verticesList[i + 2]; 193 194 covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j] * p[k] + 195 q[j] * q[k] + r[j] * r[k]) - center[j] * center[k]; 196 } 195 197 } 196 198 } 197 } 198 PRINTF(3)("-- Calculated Covariance\n"); 199 200 201 PRINTF(0)("\nVertex Data:\n"); 199 PRINTF(3)("-- Calculated Covariance\n"); 200 } 201 else if( mode == 1) 202 { 203 for( int i = 0; i < length; i+=3) /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/ 204 { 205 p = verticesList[i]; 206 q = verticesList[i + 1]; 207 r = verticesList[i + 2]; 208 209 centroid[i] = (p + q + r) / 3.0f; 210 center += centroid[i]; 211 } 212 center /= length; 213 214 for( int j = 0; j < 3; ++j) 215 { 216 for( int k = 0; k < 3; ++k) 217 { 218 for( int i = 0; i < length; i+=3) 219 { 220 p = verticesList[i]; 221 q = verticesList[i +1]; 222 r = verticesList[i + 2]; 223 224 covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k]; 225 } 226 covariance[j][k] /= (3.0f * length); 227 } 228 } 229 PRINTF(3)("-- Calculated Covariance\n"); 230 } 231 else if( mode == 2) 232 { 233 /* fist compute all the convex hull face/facelets and centroids */ 234 for(int i = 0; i < length; i+=3) /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/ 235 { 236 p = verticesList[i]; 237 q = verticesList[i + 1]; 238 r = verticesList[i + 2]; 239 240 t1 = p - q; t2 = p - r; 241 242 /* finding the facelet surface via cross-product */ 243 facelet[i] = 0.5f * fabs( t1.cross(t2).len() ); 244 /* update the entire convex hull surface */ 245 face += facelet[i]; 246 247 /* calculate the cetroid of the hull triangles */ 248 centroid[i] = (p + q + r) * 1/3; 249 /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */ 250 center += centroid[i] * facelet[i]; 251 } 252 /* take the average of the centroid sum */ 253 center /= face; 254 PRINTF(3)("-- Calculated Center\n"); 255 256 for( int j = 0; j < 3; ++j) 257 { 258 for( int k = 0; k < 3; ++k) 259 { 260 for( int i = 0; i < length; i+=3) 261 { 262 p = verticesList[i]; 263 q = verticesList[i +1]; 264 r = verticesList[i + 2]; 265 266 covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k]; 267 } 268 covariance[j][k] /= (3.0f * length); 269 } 270 } 271 PRINTF(3)("-- Calculated Covariance\n"); 272 } 273 else 274 { 275 for( int i = 0; i < length; ++i) /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/ 276 { 277 center += verticesList[i]; 278 } 279 center /= length; 280 281 for( int j = 0; j < 3; ++j) 282 { 283 for( int k = 0; k < 3; ++k) 284 { 285 for( int i = 0; i < length; i+=3) 286 { 287 p = verticesList[i]; 288 q = verticesList[i +1]; 289 r = verticesList[i + 2]; 290 291 covariance[j][k] = p[j] * p[k] + q[j] * q[k] + r[j] + r[k]; 292 } 293 covariance[j][k] /= (3.0f * length); 294 } 295 } 296 PRINTF(3)("-- Calculated Covariance\n"); 297 } 298 299 PRINTF(3)("\nVertex Data:\n"); 202 300 for(int i = 0; i < length; i++) 203 301 { 204 302 //PRINTF(0)("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]); 205 PRINTF(0)("vertex %i: %f, %f, %f\n", i, box->vertices[i][0], box->vertices[i][1], box->vertices[i][2]); 206 } 303 PRINTF(3)("vertex %i: %f, %f, %f\n", i, box->vertices[i][0], box->vertices[i][1], box->vertices[i][2]); 304 } 305 207 306 208 307 // PRINTF(3)("\nCovariance Matrix:\n"); … … 463 562 if( !(drawMode & DRAW_SINGLE && depth != 0)) 464 563 { 465 glBegin(GL_LINE_STRIP);564 //glBegin(GL_LINE_STRIP); 466 565 for(int i = 0; i < this->bvElement->numOfVertices; ++i) 467 566 { 468 567 glPushMatrix(); 469 glMatrixMode(GL_MODELVIEW);470 //glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]);568 //glMatrixMode(GL_MODELVIEW); 569 //glVertex3f(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]); 471 570 glTranslatef(this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]); 472 571 gluSphere(this->sphereObj, 1, 10, 10); 473 //PRINTF(0)("v(%f, %f, %f)\n", this-> vertices[i][0], this->vertices[i][1], this->vertices[i][2]);572 //PRINTF(0)("v(%f, %f, %f)\n", this->bvElement->vertices[i][0], this->bvElement->vertices[i][1], this->bvElement->vertices[i][2]); 474 573 glPopMatrix(); 475 574 } 476 glEnd();575 //glEnd(); 477 576 } 478 577 }
Note: See TracChangeset
for help on using the changeset viewer.