[4541] | 1 | /* |
---|
| 2 | orxonox - the future of 3D-vertical-scrollers |
---|
| 3 | |
---|
| 4 | Copyright (C) 2004 orx |
---|
| 5 | |
---|
| 6 | This program is free software; you can redistribute it and/or modify |
---|
| 7 | it under the terms of the GNU General Public License as published by |
---|
| 8 | the Free Software Foundation; either version 2, or (at your option) |
---|
| 9 | any later version. |
---|
| 10 | |
---|
| 11 | ### File Specific: |
---|
| 12 | main-programmer: Patrick Boenzli |
---|
| 13 | co-programmer: ... |
---|
| 14 | */ |
---|
| 15 | |
---|
| 16 | #define DEBUG_SPECIAL_MODULE DEBUG_MODULE_COLLISION |
---|
| 17 | |
---|
| 18 | #include "obb_tree_node.h" |
---|
[4542] | 19 | #include "list.h" |
---|
| 20 | #include "obb.h" |
---|
[4544] | 21 | #include "vector.h" |
---|
[4550] | 22 | #include "abstract_model.h" |
---|
[4541] | 23 | |
---|
[4543] | 24 | #include <math.h> |
---|
| 25 | |
---|
[4572] | 26 | |
---|
| 27 | #define WANT_STREAM |
---|
| 28 | #define WANT_MATH |
---|
| 29 | #define WANT_FSTREAM |
---|
| 30 | |
---|
| 31 | |
---|
| 32 | #include "include.h" |
---|
| 33 | #include "newmat.h" |
---|
| 34 | #include "newmatap.h" |
---|
| 35 | #include "newmatio.h" |
---|
| 36 | |
---|
| 37 | |
---|
| 38 | |
---|
| 39 | |
---|
[4541] | 40 | using namespace std; |
---|
| 41 | |
---|
| 42 | |
---|
| 43 | /** |
---|
| 44 | \brief standard constructor |
---|
| 45 | */ |
---|
| 46 | OBBTreeNode::OBBTreeNode () |
---|
| 47 | { |
---|
| 48 | this->setClassID(CL_OBB_TREE_NODE, "OBBTreeNode"); |
---|
| 49 | |
---|
| 50 | } |
---|
| 51 | |
---|
| 52 | |
---|
| 53 | /** |
---|
| 54 | \brief standard deconstructor |
---|
| 55 | |
---|
| 56 | */ |
---|
| 57 | OBBTreeNode::~OBBTreeNode () |
---|
| 58 | { |
---|
| 59 | // delete what has to be deleted here |
---|
| 60 | } |
---|
| 61 | |
---|
| 62 | |
---|
[4542] | 63 | |
---|
| 64 | /** |
---|
| 65 | \brief creates a new BVTree or BVTree partition |
---|
| 66 | \param depth: the depth of the tree |
---|
| 67 | \param verticesList: the list of vertices of the object - each vertices triple is interpreted as a triangle |
---|
| 68 | */ |
---|
[4544] | 69 | void OBBTreeNode::spawnBVTree(const int depth, sVec3D *verticesList, const int length) |
---|
[4542] | 70 | { |
---|
[4557] | 71 | this->bvElement = this->createBox(); |
---|
[4560] | 72 | this->calculateBoxAttributes(this->bvElement, verticesList, length); |
---|
| 73 | this->forkBox(this->bvElement); |
---|
[4557] | 74 | } |
---|
| 75 | |
---|
| 76 | |
---|
| 77 | OBB* OBBTreeNode::createBox() |
---|
| 78 | { |
---|
| 79 | return new OBB(); |
---|
| 80 | } |
---|
| 81 | |
---|
| 82 | |
---|
[4560] | 83 | void OBBTreeNode::calculateBoxAttributes(OBB* box, sVec3D* verticesList, int length) |
---|
[4557] | 84 | { |
---|
[4543] | 85 | float facelet[length]; //!< surface area of the i'th triangle of the convex hull |
---|
| 86 | float face; //!< surface area of the entire convex hull |
---|
[4545] | 87 | Vector centroid[length]; //!< centroid of the i'th convex hull |
---|
[4557] | 88 | Vector center; //!< the center of the entire hull |
---|
[4544] | 89 | Vector p, q, r; //!< holder of the polygon data, much more conveniant to work with Vector than sVec3d |
---|
[4545] | 90 | Vector t1, t2; //!< temporary values |
---|
[4562] | 91 | float covariance[3][3]; //!< the covariance matrix |
---|
[4545] | 92 | |
---|
[4553] | 93 | this->numOfVertices = length; |
---|
| 94 | this->vertices = verticesList; |
---|
| 95 | |
---|
[4562] | 96 | |
---|
[4545] | 97 | /* fist compute all the convex hull face/facelets and centroids */ |
---|
| 98 | for(int i = 0; i < length; i+=3) /* FIX-ME-QUICK: hops of 3, array indiscontinuity*/ |
---|
[4542] | 99 | { |
---|
[4553] | 100 | p = verticesList[i]; |
---|
| 101 | q = verticesList[i +1]; |
---|
| 102 | r = verticesList[i + 2]; |
---|
[4544] | 103 | |
---|
| 104 | t1 = p - q; t2 = p - r; |
---|
| 105 | |
---|
| 106 | /* finding the facelet surface via cross-product */ |
---|
| 107 | facelet[i] = 0.5f * fabs( t1.cross(t2).len() ); |
---|
| 108 | /* update the entire convex hull surface */ |
---|
| 109 | face += facelet[i]; |
---|
[4545] | 110 | |
---|
| 111 | /* calculate the cetroid of the hull triangles */ |
---|
| 112 | centroid[i] = (p + q + r) * 1/3; |
---|
| 113 | /* now calculate the centroid of the entire convex hull, weighted average of triangle centroids */ |
---|
[4557] | 114 | center += centroid[i] * facelet[i]; |
---|
[4542] | 115 | } |
---|
[4545] | 116 | /* take the average of the centroid sum */ |
---|
[4557] | 117 | center /= face; |
---|
[4562] | 118 | |
---|
| 119 | |
---|
[4545] | 120 | |
---|
| 121 | /* now calculate the covariance matrix - if not written in three for-loops, it would compute faster: minor */ |
---|
| 122 | for(int j = 0; j < 3; ++j) |
---|
| 123 | { |
---|
| 124 | for(int k = 0; k < 3; ++k) |
---|
| 125 | { |
---|
| 126 | for(int i = 0; i < length; i+=3) |
---|
| 127 | { |
---|
[4553] | 128 | p = verticesList[i]; |
---|
| 129 | q = verticesList[i +1]; |
---|
| 130 | r = verticesList[i + 2]; |
---|
[4544] | 131 | |
---|
[4545] | 132 | covariance[j][k] = facelet[i] / (12.0f * face) * (9.0f * centroid[i][j] * centroid[i][k] + p[j]* p[k] + |
---|
[4557] | 133 | q[j] * q[k] + r[j]*r[k]) - center[j] * center[k]; |
---|
[4545] | 134 | } |
---|
| 135 | } |
---|
| 136 | } |
---|
[4562] | 137 | |
---|
[4578] | 138 | printf("\nVertex Data:\n"); |
---|
| 139 | for(int i = 0; i < length; i++) |
---|
| 140 | { |
---|
| 141 | printf("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]); |
---|
| 142 | } |
---|
[4557] | 143 | |
---|
[4578] | 144 | printf("\nCovariance Matrix:\n"); |
---|
[4553] | 145 | for(int j = 0; j < 3; ++j) |
---|
[4551] | 146 | { |
---|
| 147 | printf(" |"); |
---|
| 148 | for(int k = 0; k < 3; ++k) |
---|
| 149 | { |
---|
| 150 | printf(" \b%f ", covariance[j][k]); |
---|
| 151 | } |
---|
| 152 | printf(" |\n"); |
---|
| 153 | } |
---|
[4560] | 154 | printf("center: %f, %f, %f\n\n", center.x, center.y, center.z); |
---|
[4553] | 155 | |
---|
[4560] | 156 | |
---|
| 157 | for(int i = 0; i < 3; ++i) |
---|
| 158 | { |
---|
[4557] | 159 | |
---|
[4560] | 160 | box->covarianceMatrix[i][0] = covariance[i][0]; |
---|
| 161 | box->covarianceMatrix[i][1] = covariance[i][1]; |
---|
| 162 | box->covarianceMatrix[i][3] = covariance[i][2]; |
---|
| 163 | } |
---|
| 164 | *box->center = center; |
---|
[4557] | 165 | |
---|
| 166 | |
---|
| 167 | /* now getting spanning vectors of the sub-space: |
---|
| 168 | the eigenvectors of a symmertric matrix, such as the |
---|
| 169 | covarience matrix are mutually orthogonal. |
---|
| 170 | after normalizing them, they can be used as a the basis |
---|
| 171 | vectors |
---|
| 172 | */ |
---|
[4573] | 173 | Matrix V(3,3); //!< for eigenvectors |
---|
| 174 | DiagonalMatrix D(3); //!< for eigenvalues |
---|
| 175 | SymmetricMatrix C(3); //!< for the covariance symmetrical matrix |
---|
[4576] | 176 | Vector** axis = new Vector*[3]; //!< the references to the obb axis |
---|
[4572] | 177 | |
---|
[4573] | 178 | C(1,1) = covariance[0][0]; |
---|
| 179 | C(1,2) = covariance[0][1]; |
---|
| 180 | C(1,3) = covariance[0][2]; |
---|
| 181 | C(2,1) = covariance[1][0]; |
---|
| 182 | C(2,2) = covariance[1][1]; |
---|
| 183 | C(2,3) = covariance[1][2]; |
---|
| 184 | C(3,1) = covariance[2][0]; |
---|
| 185 | C(3,2) = covariance[2][1]; |
---|
| 186 | C(3,3) = covariance[2][2]; |
---|
[4572] | 187 | |
---|
[4573] | 188 | Jacobi(C, D, V); /* do the jacobi decomposition */ |
---|
[4572] | 189 | |
---|
[4573] | 190 | printf("we got a result! YES: \n"); |
---|
[4572] | 191 | |
---|
[4573] | 192 | for(int j = 1; j < 4; ++j) |
---|
| 193 | { |
---|
| 194 | printf(" |"); |
---|
| 195 | for(int k = 1; k < 4; ++k) |
---|
| 196 | { |
---|
| 197 | printf(" \b%f ", V(j, k)); |
---|
| 198 | } |
---|
| 199 | printf(" |\n"); |
---|
| 200 | } |
---|
| 201 | |
---|
[4576] | 202 | axis[0] = new Vector(V(1, 1), V(2, 1), V(3, 1)); |
---|
| 203 | axis[1] = new Vector(V(1, 2), V(2, 2), V(3, 2)); |
---|
| 204 | axis[2] = new Vector(V(1, 3), V(2, 3), V(3, 3)); |
---|
| 205 | box->axis = axis; |
---|
| 206 | |
---|
| 207 | printf("eigenvector: %f, %f, %f\n", V(1, 1), V(2, 1), V(3, 1)); |
---|
| 208 | printf("eigenvector: %f, %f, %f\n", V(1, 2), V(2, 2), V(3, 2)); |
---|
| 209 | printf("eigenvector: %f, %f, %f\n", V(1, 3), V(2, 3), V(3, 3)); |
---|
[4573] | 210 | |
---|
[4576] | 211 | |
---|
| 212 | /* now get the axis length */ |
---|
[4578] | 213 | Line ax[3]; //!< the axis |
---|
| 214 | float* halfLength = new float[3]; //!< half length of the axis |
---|
| 215 | float tmpLength; //!< tmp save point for the length |
---|
[4576] | 216 | |
---|
[4578] | 217 | ax[0].r = *box->center; ax[0].a = *box->axis[0]; |
---|
| 218 | ax[1].r = *box->center; ax[1].a = *box->axis[1]; |
---|
| 219 | ax[2].r = *box->center; ax[2].a = *box->axis[2]; |
---|
[4576] | 220 | |
---|
[4578] | 221 | for(int i = 0; i < 3; ++i) |
---|
| 222 | { |
---|
| 223 | for(int j = 0; j < length; ++j) |
---|
| 224 | { |
---|
| 225 | tmpLength = ax[i].distancePoint(vertices[j]); |
---|
| 226 | if( tmpLength > halfLength[i]) |
---|
| 227 | halfLength[i] = tmpLength; |
---|
| 228 | } |
---|
| 229 | } |
---|
| 230 | |
---|
| 231 | printf("we got length: \n"); |
---|
| 232 | for(int i = 0; i < 3; ++i) |
---|
| 233 | printf("length[%i] = %f\n", i, halfLength[i]); |
---|
[4542] | 234 | } |
---|
| 235 | |
---|
| 236 | |
---|
[4557] | 237 | void OBBTreeNode::forkBox(OBB* box) |
---|
| 238 | { |
---|
| 239 | /* get the longest axis of the box */ |
---|
| 240 | float aLength = -1.0f; |
---|
| 241 | int axisNr = 0; |
---|
| 242 | for(int i = 0; i < 3; ++i) |
---|
| 243 | { |
---|
[4576] | 244 | if( aLength < box->axis[i]->len()) |
---|
[4557] | 245 | { |
---|
[4576] | 246 | aLength = box->axis[i]->len(); |
---|
[4557] | 247 | axisNr = i; |
---|
| 248 | } |
---|
| 249 | } |
---|
| 250 | |
---|
| 251 | /* get the closest vertex near the center */ |
---|
| 252 | |
---|
| 253 | } |
---|
| 254 | |
---|
| 255 | |
---|
[4542] | 256 | void OBBTreeNode::collideWith(const BVTree &tree) |
---|
| 257 | {} |
---|
| 258 | |
---|
| 259 | |
---|
| 260 | void OBBTreeNode::drawBV(int currentDepth, const int depth) const |
---|
[4553] | 261 | { |
---|
| 262 | glColor3f(1.0, 1.0, 1.0); |
---|
| 263 | glBegin(GL_TRIANGLES); |
---|
| 264 | for(int i = 0; i < this->numOfVertices; ++i) |
---|
| 265 | { |
---|
| 266 | glVertex3f(this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]); |
---|
| 267 | //printf("v(%f, %f, %f)\n", this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]); |
---|
| 268 | } |
---|
| 269 | glEnd(); |
---|
| 270 | } |
---|
[4542] | 271 | |
---|
| 272 | |
---|
| 273 | void OBBTreeNode::drawBVPolygon(int currentDepth, const int depth) const |
---|
[4557] | 274 | { |
---|
| 275 | this->bvElement->axis; |
---|
| 276 | |
---|
[4560] | 277 | //glBegin(GL_TRIANGLE); |
---|
| 278 | //glVertex3f(this->bvElement->center ); |
---|
| 279 | //glEnd(); |
---|
[4557] | 280 | } |
---|
[4542] | 281 | |
---|
| 282 | |
---|
| 283 | void OBBTreeNode::drawBVBlended(int currentDepth, const int depth) const |
---|
| 284 | {} |
---|
[4568] | 285 | |
---|
| 286 | |
---|
| 287 | void OBBTreeNode::debug() |
---|
| 288 | { |
---|
| 289 | |
---|
| 290 | /* |
---|
| 291 | for(int i = 0; i < length; i++) |
---|
| 292 | { |
---|
| 293 | printf("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]); |
---|
| 294 | } |
---|
| 295 | */ |
---|
| 296 | } |
---|