polytope.cpp
Engine/source/collision/polytope.cpp
Detailed Description
1 2//----------------------------------------------------------------------------- 3// Copyright (c) 2012 GarageGames, LLC 4// 5// Permission is hereby granted, free of charge, to any person obtaining a copy 6// of this software and associated documentation files (the "Software"), to 7// deal in the Software without restriction, including without limitation the 8// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 9// sell copies of the Software, and to permit persons to whom the Software is 10// furnished to do so, subject to the following conditions: 11// 12// The above copyright notice and this permission notice shall be included in 13// all copies or substantial portions of the Software. 14// 15// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 20// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 21// IN THE SOFTWARE. 22//----------------------------------------------------------------------------- 23 24#include "platform/platform.h" 25#include "math/mMath.h" 26#include "collision/collision.h" 27#include "collision/polytope.h" 28 29//---------------------------------------------------------------------------- 30 31Polytope::Polytope() 32{ 33 VECTOR_SET_ASSOCIATION(mEdgeList); 34 VECTOR_SET_ASSOCIATION(mFaceList); 35 VECTOR_SET_ASSOCIATION(mVertexList); 36 VECTOR_SET_ASSOCIATION(mVolumeList); 37 38 mVertexList.reserve(100); 39 mFaceList.reserve(200); 40 mEdgeList.reserve(100); 41 mVolumeList.reserve(20); 42 sideCount = 0; 43} 44 45 46//---------------------------------------------------------------------------- 47// Box should be axis aligned in the transform space provided. 48 49void Polytope::buildBox(const MatrixF& transform,const Box3F& box) 50{ 51 // Box is assumed to be axis aligned in the source space. 52 // Transform into geometry space 53 Point3F xvec,yvec,zvec,min; 54 transform.getColumn(0,&xvec); 55 xvec *= box.len_x(); 56 transform.getColumn(1,&yvec); 57 yvec *= box.len_y(); 58 transform.getColumn(2,&zvec); 59 zvec *= box.len_z(); 60 transform.mulP(box.minExtents,&min); 61 62 // Initial vertices 63 mVertexList.setSize(8); 64 mVertexList[0].point = min; 65 mVertexList[1].point = min + yvec; 66 mVertexList[2].point = min + xvec + yvec; 67 mVertexList[3].point = min + xvec; 68 mVertexList[4].point = mVertexList[0].point + zvec; 69 mVertexList[5].point = mVertexList[1].point + zvec; 70 mVertexList[6].point = mVertexList[2].point + zvec; 71 mVertexList[7].point = mVertexList[3].point + zvec; 72 S32 i; 73 for (i = 0; i < 8; i++) 74 mVertexList[i].side = 0; 75 76 // Initial faces 77 mFaceList.setSize(6); 78 for (S32 f = 0; f < 6; f++) { 79 Face& face = mFaceList[f]; 80 face.original = true; 81 face.vertex = 0; 82 } 83 84 mFaceList[0].plane.set(mVertexList[0].point,xvec); 85 mFaceList[0].plane.invert(); 86 mFaceList[1].plane.set(mVertexList[2].point,yvec); 87 mFaceList[2].plane.set(mVertexList[2].point,xvec); 88 mFaceList[3].plane.set(mVertexList[0].point,yvec); 89 mFaceList[3].plane.invert(); 90 mFaceList[4].plane.set(mVertexList[0].point,zvec); 91 mFaceList[4].plane.invert(); 92 mFaceList[5].plane.set(mVertexList[4].point,zvec); 93 94 // Initial edges 95 mEdgeList.setSize(12); 96 Edge* edge = mEdgeList.begin(); 97 S32 nextEdge = 0; 98 for (i = 0; i < 4; i++) { 99 S32 n = (i == 3)? 0: i + 1; 100 S32 p = (i == 0)? 3: i - 1; 101 edge->vertex[0] = i; 102 edge->vertex[1] = n; 103 edge->face[0] = i; 104 edge->face[1] = 4; 105 edge->next = ++nextEdge; 106 edge++; 107 edge->vertex[0] = 4 + i; 108 edge->vertex[1] = 4 + n; 109 edge->face[0] = i; 110 edge->face[1] = 5; 111 edge->next = ++nextEdge; 112 edge++; 113 edge->vertex[0] = i; 114 edge->vertex[1] = 4 + i; 115 edge->face[0] = i; 116 edge->face[1] = p; 117 edge->next = ++nextEdge; 118 edge++; 119 } 120 edge[-1].next = -1; 121 122 // Volume 123 mVolumeList.setSize(1); 124 Volume& volume = mVolumeList.last(); 125 volume.edgeList = 0; 126 volume.material = -1; 127 volume.object = 0; 128 sideCount = 0; 129} 130 131 132//---------------------------------------------------------------------------- 133 134void Polytope::intersect(SimObject* object,const BSPNode* root) 135{ 136 AssertFatal(mVolumeList.size() > 0,"Polytope::intersect: Missing initial volume"); 137 138 // Always clips the first volume against the BSP 139 VolumeStack stack; 140 stack.reserve(50); 141 stack.increment(); 142 stack.last().edgeList = mVolumeList[0].edgeList; 143 stack.last().node = root; 144 145 while (!stack.empty()) { 146 StackElement volume = stack.last(); 147 stack.pop_back(); 148 149 // If it's a solid node, export the volume 150 const BSPNode* node = volume.node; 151 if (!node->backNode && !node->frontNode) { 152 mVolumeList.increment(); 153 Volume& vol = mVolumeList.last(); 154 vol.object = object; 155 vol.material = node->material; 156 vol.edgeList = volume.edgeList; 157 continue; 158 } 159 160 // New front and back faces 161 mFaceList.increment(2); 162 Face& frontFace = mFaceList.last(); 163 Face& backFace = *(&frontFace - 1); 164 165 backFace.original = frontFace.original = false; 166 backFace.vertex = frontFace.vertex = 0; 167 backFace.plane = frontFace.plane = node->plane; 168 backFace.plane.invert(); 169 170 // New front and back volumes 171 StackElement frontVolume,backVolume; 172 frontVolume.edgeList = backVolume.edgeList = -1; 173 174 const PlaneF& plane = node->plane; 175 S32 startVertex = mVertexList.size(); 176 177 // Test & clip all the edges 178 S32 sideBase = ++sideCount << 1; 179 for (S32 i = volume.edgeList; i >= 0; i = mEdgeList[i].next) { 180 181 // Copy into tmp first to avoid problems with the array. 182 Edge edge = mEdgeList[i]; 183 184 Vertex& v0 = mVertexList[edge.vertex[0]]; 185 if (v0.side < sideBase) 186 v0.side = sideBase + ((plane.distToPlane(v0.point) >= 0)? 0: 1); 187 Vertex& v1 = mVertexList[edge.vertex[1]]; 188 if (v1.side < sideBase) 189 v1.side = sideBase + ((plane.distToPlane(v1.point) >= 0)? 0: 1); 190 191 if (v0.side != v1.side) { 192 S32 s = v0.side - sideBase; 193 intersect(plane,v0.point,v1.point); 194 195 // Split the edge into each volume 196 mEdgeList.increment(2); 197 Edge& ev0 = mEdgeList.last(); 198 ev0.next = frontVolume.edgeList; 199 frontVolume.edgeList = mEdgeList.size() - 1; 200 201 Edge& ev1 = *(&ev0 - 1); 202 ev1.next = backVolume.edgeList; 203 backVolume.edgeList = frontVolume.edgeList - 1; 204 205 ev0.vertex[0] = edge.vertex[s]; 206 ev1.vertex[0] = edge.vertex[s ^ 1]; 207 ev0.vertex[1] = ev1.vertex[1] = mVertexList.size() - 1; 208 ev0.face[0] = ev1.face[0] = edge.face[0]; 209 ev0.face[1] = ev1.face[1] = edge.face[1]; 210 211 // Add new edges on the plane, one to each volume 212 for (S32 f = 0; f < 2; f++) { 213 Face& face = mFaceList[edge.face[f]]; 214 if (face.vertex < startVertex) 215 face.vertex = mVertexList.size() - 1; 216 else { 217 mEdgeList.increment(2); 218 Edge& ep0 = mEdgeList.last(); 219 ep0.next = frontVolume.edgeList; 220 frontVolume.edgeList = mEdgeList.size() - 1; 221 222 Edge& ep1 = *(&ep0 - 1); 223 ep1.next = backVolume.edgeList; 224 backVolume.edgeList = frontVolume.edgeList - 1; 225 226 ep1.vertex[0] = ep0.vertex[0] = face.vertex; 227 ep1.vertex[1] = ep0.vertex[1] = mVertexList.size() - 1; 228 ep1.face[0] = ep0.face[0] = edge.face[f]; 229 ep1.face[1] = mFaceList.size() - 1; 230 ep0.face[1] = ep1.face[1] - 1; 231 } 232 } 233 } 234 else 235 if (v0.side == sideBase) { 236 mEdgeList.push_back(edge); 237 Edge& ne = mEdgeList.last(); 238 ne.next = frontVolume.edgeList; 239 frontVolume.edgeList = mEdgeList.size() - 1; 240 } 241 else { 242 mEdgeList.push_back(edge); 243 Edge& ne = mEdgeList.last(); 244 ne.next = backVolume.edgeList; 245 backVolume.edgeList = mEdgeList.size() - 1; 246 } 247 } 248 249 // Push the front and back nodes 250 if (node->frontNode && frontVolume.edgeList >= 0) { 251 frontVolume.node = node->frontNode; 252 stack.push_back(frontVolume); 253 } 254 if (node->backNode && backVolume.edgeList >= 0) { 255 backVolume.node = node->backNode; 256 stack.push_back(backVolume); 257 } 258 } 259} 260 261 262//---------------------------------------------------------------------------- 263 264bool Polytope::intersect(const PlaneF& plane,const Point3F& sp,const Point3F& ep) 265{ 266 // If den == 0 then the line and plane are parallel. 267 F32 den; 268 Point3F dt = ep - sp; 269 if ((den = plane.x * dt.x + plane.y * dt.y + plane.z * dt.z) == 0) 270 return false; 271 272 mVertexList.increment(); 273 Vertex& v = mVertexList.last(); 274 F32 s = -(plane.x * sp.x + plane.y * sp.y + plane.z * sp.z + plane.d) / den; 275 v.point.x = sp.x + dt.x * s; 276 v.point.y = sp.y + dt.y * s; 277 v.point.z = sp.z + dt.z * s; 278 v.side = 0; 279 return true; 280} 281 282//---------------------------------------------------------------------------- 283 284void Polytope::extrudeFace(S32 faceIdx,const VectorF& vec,Polytope* out) 285{ 286 // Assumes the face belongs to the first volume. 287 out->mVertexList.clear(); 288 out->mFaceList.clear(); 289 out->mEdgeList.clear(); 290 out->mVolumeList.clear(); 291 sideCount++; 292 293 // Front & end faces 294 Face nface; 295 nface.original = true; 296 nface.vertex = 0; 297 nface.plane = mFaceList[faceIdx].plane; 298 out->mFaceList.setSize(2); 299 out->mFaceList[0] = out->mFaceList[1] = nface; 300 out->mFaceList[0].plane.invert(); 301 302 for (S32 e = mVolumeList[0].edgeList; e >= 0; e = mEdgeList[e].next) { 303 Edge& edge = mEdgeList[e]; 304 if (edge.face[0] == faceIdx || edge.face[1] == faceIdx) { 305 306 // Build face for this edge 307 // Should think about calulating the plane 308 S32 fi = out->mFaceList.size(); 309 out->mFaceList.push_back(nface); 310 311 // Reserve 4 entries to make sure the ve[] pointers 312 // into the list don't get invalidated. 313 out->mEdgeList.reserve(out->mEdgeList.size() + 4); 314 Edge* ve[2]; 315 316 // Build edges for each vertex 317 for (S32 v = 0; v < 2; v++) { 318 if (mVertexList[edge.vertex[v]].side < sideCount) { 319 mVertexList[edge.vertex[v]].side = sideCount + out->mEdgeList.size(); 320 321 out->mVertexList.increment(2); 322 out->mVertexList.end()[-1] = 323 out->mVertexList.end()[-2] = 324 mVertexList[edge.vertex[v]]; 325 out->mVertexList.last().point += vec; 326 327 out->mEdgeList.increment(); 328 Edge& ne = out->mEdgeList.last(); 329 ne.next = out->mEdgeList.size(); 330 ne.vertex[1] = out->mVertexList.size() - 1; 331 ne.vertex[0] = ne.vertex[1] - 1; 332 ne.face[0] = ne.face[1] = -1; 333 ve[v] = ≠ 334 } 335 else { 336 S32 ei = mVertexList[edge.vertex[v]].side - sideCount; 337 ve[v] = &out->mEdgeList[ei]; 338 } 339 340 // Edge should share this face 341 if (ve[v]->face[0] == -1) 342 ve[v]->face[0] = fi; 343 else 344 ve[v]->face[1] = fi; 345 } 346 347 // Build parallel edges 348 out->mEdgeList.increment(2); 349 for (S32 i = 0; i < 2; i++ ) { 350 Edge& ne = out->mEdgeList.end()[i - 2]; 351 ne.next = out->mEdgeList.size() - 1 + i; 352 ne.vertex[0] = ve[0]->vertex[i]; 353 ne.vertex[1] = ve[1]->vertex[i]; 354 ne.face[0] = i; 355 ne.face[1] = fi; 356 } 357 } 358 } 359 360 out->mEdgeList.last().next = -1; 361 out->mVolumeList.increment(); 362 Volume& nv = out->mVolumeList.last(); 363 nv.edgeList = 0; 364 nv.object = 0; 365 nv.material = -1; 366} 367 368 369//---------------------------------------------------------------------------- 370 371bool Polytope::findCollision(const VectorF& vec,Polytope::Collision *best) 372{ 373 if (mVolumeList.size() <= 1) 374 return false; 375 if (!best->object) 376 best->distance = 1.0E30f; 377 S32 bestVertex = -1; 378 Polytope::Volume* bestVolume = NULL; 379 sideCount++; 380 381 // Find the closest point 382 for (Volume* vol = mVolumeList.begin() + 1; 383 vol < mVolumeList.end(); vol++) { 384 for (S32 e = vol->edgeList; e >= 0; e = mEdgeList[e].next) { 385 Edge& edge = mEdgeList[e]; 386 if (mFaceList[edge.face[0]].original && 387 mFaceList[edge.face[1]].original) 388 continue; 389 for (S32 v = 0; v < 2; v++) { 390 S32 vi = edge.vertex[v]; 391 Vertex& vr = mVertexList[vi]; 392 if (vr.side != sideCount) { 393 vr.side = sideCount; 394 F32 dist = mDot(vr.point,vec); 395 if (dist < best->distance) { 396 best->distance = dist; 397 bestVertex = vi; 398 bestVolume = vol; 399 } 400 } 401 } 402 } 403 } 404 405 if (bestVertex == -1) 406 return false; 407 408 // Fill in the return value 409 best->point = mVertexList[bestVertex].point; 410 best->object = bestVolume->object; 411 best->material = bestVolume->material; 412 413 // Pick the best face 414 F32 bestFaceDot = 1; 415 for (S32 e = bestVolume->edgeList; e >= 0; e = mEdgeList[e].next) { 416 Edge& edge = mEdgeList[e]; 417 if (edge.vertex[0] == bestVertex || edge.vertex[1] == bestVertex) { 418 for (S32 f = 0; f < 2; f++) { 419 Face& tf = mFaceList[edge.face[f]]; 420 F32 fd = mDot(tf.plane,vec); 421 if (fd < bestFaceDot) { 422 bestFaceDot = fd; 423 best->plane = tf.plane; 424 } 425 } 426 } 427 } 428 return true; 429} 430 431