[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 | |
---|
[4557] | 138 | |
---|
[4551] | 139 | printf("Covariance Matrix:\n"); |
---|
[4553] | 140 | for(int j = 0; j < 3; ++j) |
---|
[4551] | 141 | { |
---|
| 142 | printf(" |"); |
---|
| 143 | for(int k = 0; k < 3; ++k) |
---|
| 144 | { |
---|
| 145 | printf(" \b%f ", covariance[j][k]); |
---|
| 146 | } |
---|
| 147 | printf(" |\n"); |
---|
| 148 | } |
---|
[4560] | 149 | printf("center: %f, %f, %f\n\n", center.x, center.y, center.z); |
---|
[4553] | 150 | |
---|
[4560] | 151 | |
---|
| 152 | for(int i = 0; i < 3; ++i) |
---|
| 153 | { |
---|
[4557] | 154 | |
---|
[4560] | 155 | box->covarianceMatrix[i][0] = covariance[i][0]; |
---|
| 156 | box->covarianceMatrix[i][1] = covariance[i][1]; |
---|
| 157 | box->covarianceMatrix[i][3] = covariance[i][2]; |
---|
| 158 | } |
---|
| 159 | *box->center = center; |
---|
[4557] | 160 | |
---|
| 161 | |
---|
| 162 | /* now getting spanning vectors of the sub-space: |
---|
| 163 | the eigenvectors of a symmertric matrix, such as the |
---|
| 164 | covarience matrix are mutually orthogonal. |
---|
| 165 | after normalizing them, they can be used as a the basis |
---|
| 166 | vectors |
---|
| 167 | */ |
---|
| 168 | |
---|
[4573] | 169 | Matrix V(3,3); //!< for eigenvectors |
---|
| 170 | DiagonalMatrix D(3); //!< for eigenvalues |
---|
| 171 | SymmetricMatrix C(3); //!< for the covariance symmetrical matrix |
---|
| 172 | Vector** axis; //!< the references to the obb axis |
---|
[4572] | 173 | |
---|
[4573] | 174 | C(1,1) = covariance[0][0]; |
---|
| 175 | C(1,2) = covariance[0][1]; |
---|
| 176 | C(1,3) = covariance[0][2]; |
---|
| 177 | C(2,1) = covariance[1][0]; |
---|
| 178 | C(2,2) = covariance[1][1]; |
---|
| 179 | C(2,3) = covariance[1][2]; |
---|
| 180 | C(3,1) = covariance[2][0]; |
---|
| 181 | C(3,2) = covariance[2][1]; |
---|
| 182 | C(3,3) = covariance[2][2]; |
---|
[4572] | 183 | |
---|
[4573] | 184 | Jacobi(C, D, V); /* do the jacobi decomposition */ |
---|
[4572] | 185 | |
---|
[4573] | 186 | printf("we got a result! YES: \n"); |
---|
[4572] | 187 | |
---|
[4573] | 188 | for(int j = 1; j < 4; ++j) |
---|
| 189 | { |
---|
| 190 | printf(" |"); |
---|
| 191 | for(int k = 1; k < 4; ++k) |
---|
| 192 | { |
---|
| 193 | printf(" \b%f ", V(j, k)); |
---|
| 194 | } |
---|
| 195 | printf(" |\n"); |
---|
| 196 | } |
---|
| 197 | |
---|
| 198 | //axis1 = |
---|
| 199 | |
---|
[4542] | 200 | } |
---|
| 201 | |
---|
| 202 | |
---|
[4557] | 203 | void OBBTreeNode::forkBox(OBB* box) |
---|
| 204 | { |
---|
| 205 | /* get the longest axis of the box */ |
---|
| 206 | float aLength = -1.0f; |
---|
| 207 | int axisNr = 0; |
---|
| 208 | for(int i = 0; i < 3; ++i) |
---|
| 209 | { |
---|
[4560] | 210 | if( aLength < box->axis[i].len()) |
---|
[4557] | 211 | { |
---|
[4560] | 212 | aLength = box->axis[i].len(); |
---|
[4557] | 213 | axisNr = i; |
---|
| 214 | } |
---|
| 215 | } |
---|
| 216 | |
---|
| 217 | /* get the closest vertex near the center */ |
---|
| 218 | |
---|
| 219 | } |
---|
| 220 | |
---|
| 221 | |
---|
[4542] | 222 | void OBBTreeNode::collideWith(const BVTree &tree) |
---|
| 223 | {} |
---|
| 224 | |
---|
| 225 | |
---|
| 226 | void OBBTreeNode::drawBV(int currentDepth, const int depth) const |
---|
[4553] | 227 | { |
---|
| 228 | glColor3f(1.0, 1.0, 1.0); |
---|
| 229 | glBegin(GL_TRIANGLES); |
---|
| 230 | for(int i = 0; i < this->numOfVertices; ++i) |
---|
| 231 | { |
---|
| 232 | glVertex3f(this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]); |
---|
| 233 | //printf("v(%f, %f, %f)\n", this->vertices[i][0], this->vertices[i][1], this->vertices[i][2]); |
---|
| 234 | } |
---|
| 235 | glEnd(); |
---|
| 236 | } |
---|
[4542] | 237 | |
---|
| 238 | |
---|
| 239 | void OBBTreeNode::drawBVPolygon(int currentDepth, const int depth) const |
---|
[4557] | 240 | { |
---|
| 241 | this->bvElement->axis; |
---|
| 242 | |
---|
[4560] | 243 | //glBegin(GL_TRIANGLE); |
---|
| 244 | //glVertex3f(this->bvElement->center ); |
---|
| 245 | //glEnd(); |
---|
[4557] | 246 | } |
---|
[4542] | 247 | |
---|
| 248 | |
---|
| 249 | void OBBTreeNode::drawBVBlended(int currentDepth, const int depth) const |
---|
| 250 | {} |
---|
[4568] | 251 | |
---|
| 252 | |
---|
| 253 | void OBBTreeNode::debug() |
---|
| 254 | { |
---|
| 255 | |
---|
| 256 | /* |
---|
| 257 | for(int i = 0; i < length; i++) |
---|
| 258 | { |
---|
| 259 | printf("vertex %i: %f, %f, %f\n", i, verticesList[i][0], verticesList[i][1], verticesList[i][2]); |
---|
| 260 | } |
---|
| 261 | */ |
---|
| 262 | } |
---|