1 2//keep this enum in sync with the CPU version (in btCollidable.h) 3//written by Erwin Coumans 4 5 6#define SHAPE_CONVEX_HULL 3 7#define SHAPE_CONCAVE_TRIMESH 5 8#define TRIANGLE_NUM_CONVEX_FACES 5 9#define SHAPE_COMPOUND_OF_CONVEX_HULLS 6 10 11#define B3_MAX_STACK_DEPTH 256 12 13 14typedef unsigned int u32; 15 16///keep this in sync with btCollidable.h 17typedef struct 18{ 19 union { 20 int m_numChildShapes; 21 int m_bvhIndex; 22 }; 23 union 24 { 25 float m_radius; 26 int m_compoundBvhIndex; 27 }; 28 29 int m_shapeType; 30 int m_shapeIndex; 31 32} btCollidableGpu; 33 34#define MAX_NUM_PARTS_IN_BITS 10 35 36///b3QuantizedBvhNode is a compressed aabb node, 16 bytes. 37///Node can be used for leafnode or internal node. Leafnodes can point to 32-bit triangle index (non-negative range). 38typedef struct 39{ 40 //12 bytes 41 unsigned short int m_quantizedAabbMin[3]; 42 unsigned short int m_quantizedAabbMax[3]; 43 //4 bytes 44 int m_escapeIndexOrTriangleIndex; 45} b3QuantizedBvhNode; 46 47typedef struct 48{ 49 float4 m_aabbMin; 50 float4 m_aabbMax; 51 float4 m_quantization; 52 int m_numNodes; 53 int m_numSubTrees; 54 int m_nodeOffset; 55 int m_subTreeOffset; 56 57} b3BvhInfo; 58 59 60int getTriangleIndex(const b3QuantizedBvhNode* rootNode) 61{ 62 unsigned int x=0; 63 unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); 64 // Get only the lower bits where the triangle index is stored 65 return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); 66} 67 68int getTriangleIndexGlobal(__global const b3QuantizedBvhNode* rootNode) 69{ 70 unsigned int x=0; 71 unsigned int y = (~(x&0))<<(31-MAX_NUM_PARTS_IN_BITS); 72 // Get only the lower bits where the triangle index is stored 73 return (rootNode->m_escapeIndexOrTriangleIndex&~(y)); 74} 75 76int isLeafNode(const b3QuantizedBvhNode* rootNode) 77{ 78 //skipindex is negative (internal node), triangleindex >=0 (leafnode) 79 return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; 80} 81 82int isLeafNodeGlobal(__global const b3QuantizedBvhNode* rootNode) 83{ 84 //skipindex is negative (internal node), triangleindex >=0 (leafnode) 85 return (rootNode->m_escapeIndexOrTriangleIndex >= 0)? 1 : 0; 86} 87 88int getEscapeIndex(const b3QuantizedBvhNode* rootNode) 89{ 90 return -rootNode->m_escapeIndexOrTriangleIndex; 91} 92 93int getEscapeIndexGlobal(__global const b3QuantizedBvhNode* rootNode) 94{ 95 return -rootNode->m_escapeIndexOrTriangleIndex; 96} 97 98 99typedef struct 100{ 101 //12 bytes 102 unsigned short int m_quantizedAabbMin[3]; 103 unsigned short int m_quantizedAabbMax[3]; 104 //4 bytes, points to the root of the subtree 105 int m_rootNodeIndex; 106 //4 bytes 107 int m_subtreeSize; 108 int m_padding[3]; 109} b3BvhSubtreeInfo; 110 111 112 113 114 115 116 117typedef struct 118{ 119 float4 m_childPosition; 120 float4 m_childOrientation; 121 int m_shapeIndex; 122 int m_unused0; 123 int m_unused1; 124 int m_unused2; 125} btGpuChildShape; 126 127 128typedef struct 129{ 130 float4 m_pos; 131 float4 m_quat; 132 float4 m_linVel; 133 float4 m_angVel; 134 135 u32 m_collidableIdx; 136 float m_invMass; 137 float m_restituitionCoeff; 138 float m_frictionCoeff; 139} BodyData; 140 141 142typedef struct 143{ 144 float4 m_localCenter; 145 float4 m_extents; 146 float4 mC; 147 float4 mE; 148 149 float m_radius; 150 int m_faceOffset; 151 int m_numFaces; 152 int m_numVertices; 153 154 int m_vertexOffset; 155 int m_uniqueEdgesOffset; 156 int m_numUniqueEdges; 157 int m_unused; 158} ConvexPolyhedronCL; 159 160typedef struct 161{ 162 union 163 { 164 float4 m_min; 165 float m_minElems[4]; 166 int m_minIndices[4]; 167 }; 168 union 169 { 170 float4 m_max; 171 float m_maxElems[4]; 172 int m_maxIndices[4]; 173 }; 174} btAabbCL; 175 176#include "Bullet3Collision/BroadPhaseCollision/shared/b3Aabb.h" 177#include "Bullet3Common/shared/b3Int2.h" 178 179 180 181typedef struct 182{ 183 float4 m_plane; 184 int m_indexOffset; 185 int m_numIndices; 186} btGpuFace; 187 188#define make_float4 (float4) 189 190 191__inline 192float4 cross3(float4 a, float4 b) 193{ 194 return cross(a,b); 195 196 197// float4 a1 = make_float4(a.xyz,0.f); 198// float4 b1 = make_float4(b.xyz,0.f); 199 200// return cross(a1,b1); 201 202//float4 c = make_float4(a.y*b.z - a.z*b.y,a.z*b.x - a.x*b.z,a.x*b.y - a.y*b.x,0.f); 203 204 // float4 c = make_float4(a.y*b.z - a.z*b.y,1.f,a.x*b.y - a.y*b.x,0.f); 205 206 //return c; 207} 208 209__inline 210float dot3F4(float4 a, float4 b) 211{ 212 float4 a1 = make_float4(a.xyz,0.f); 213 float4 b1 = make_float4(b.xyz,0.f); 214 return dot(a1, b1); 215} 216 217__inline 218float4 fastNormalize4(float4 v) 219{ 220 v = make_float4(v.xyz,0.f); 221 return fast_normalize(v); 222} 223 224 225/////////////////////////////////////// 226// Quaternion 227/////////////////////////////////////// 228 229typedef float4 Quaternion; 230 231__inline 232Quaternion qtMul(Quaternion a, Quaternion b); 233 234__inline 235Quaternion qtNormalize(Quaternion in); 236 237__inline 238float4 qtRotate(Quaternion q, float4 vec); 239 240__inline 241Quaternion qtInvert(Quaternion q); 242 243 244 245 246__inline 247Quaternion qtMul(Quaternion a, Quaternion b) 248{ 249 Quaternion ans; 250 ans = cross3( a, b ); 251 ans += a.w*b+b.w*a; 252// ans.w = a.w*b.w - (a.x*b.x+a.y*b.y+a.z*b.z); 253 ans.w = a.w*b.w - dot3F4(a, b); 254 return ans; 255} 256 257__inline 258Quaternion qtNormalize(Quaternion in) 259{ 260 return fastNormalize4(in); 261// in /= length( in ); 262// return in; 263} 264__inline 265float4 qtRotate(Quaternion q, float4 vec) 266{ 267 Quaternion qInv = qtInvert( q ); 268 float4 vcpy = vec; 269 vcpy.w = 0.f; 270 float4 out = qtMul(qtMul(q,vcpy),qInv); 271 return out; 272} 273 274__inline 275Quaternion qtInvert(Quaternion q) 276{ 277 return (Quaternion)(-q.xyz, q.w); 278} 279 280__inline 281float4 qtInvRotate(const Quaternion q, float4 vec) 282{ 283 return qtRotate( qtInvert( q ), vec ); 284} 285 286__inline 287float4 transform(const float4* p, const float4* translation, const Quaternion* orientation) 288{ 289 return qtRotate( *orientation, *p ) + (*translation); 290} 291 292 293 294__inline 295float4 normalize3(const float4 a) 296{ 297 float4 n = make_float4(a.x, a.y, a.z, 0.f); 298 return fastNormalize4( n ); 299} 300 301inline void projectLocal(const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, 302const float4* dir, const float4* vertices, float* min, float* max) 303{ 304 min[0] = FLT_MAX; 305 max[0] = -FLT_MAX; 306 int numVerts = hull->m_numVertices; 307 308 const float4 localDir = qtInvRotate(orn,*dir); 309 float offset = dot(pos,*dir); 310 for(int i=0;i<numVerts;i++) 311 { 312 float dp = dot(vertices[hull->m_vertexOffset+i],localDir); 313 if(dp < min[0]) 314 min[0] = dp; 315 if(dp > max[0]) 316 max[0] = dp; 317 } 318 if(min[0]>max[0]) 319 { 320 float tmp = min[0]; 321 min[0] = max[0]; 322 max[0] = tmp; 323 } 324 min[0] += offset; 325 max[0] += offset; 326} 327 328inline void project(__global const ConvexPolyhedronCL* hull, const float4 pos, const float4 orn, 329const float4* dir, __global const float4* vertices, float* min, float* max) 330{ 331 min[0] = FLT_MAX; 332 max[0] = -FLT_MAX; 333 int numVerts = hull->m_numVertices; 334 335 const float4 localDir = qtInvRotate(orn,*dir); 336 float offset = dot(pos,*dir); 337 for(int i=0;i<numVerts;i++) 338 { 339 float dp = dot(vertices[hull->m_vertexOffset+i],localDir); 340 if(dp < min[0]) 341 min[0] = dp; 342 if(dp > max[0]) 343 max[0] = dp; 344 } 345 if(min[0]>max[0]) 346 { 347 float tmp = min[0]; 348 min[0] = max[0]; 349 max[0] = tmp; 350 } 351 min[0] += offset; 352 max[0] += offset; 353} 354 355inline bool TestSepAxisLocalA(const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, 356 const float4 posA,const float4 ornA, 357 const float4 posB,const float4 ornB, 358 float4* sep_axis, const float4* verticesA, __global const float4* verticesB,float* depth) 359{ 360 float Min0,Max0; 361 float Min1,Max1; 362 projectLocal(hullA,posA,ornA,sep_axis,verticesA, &Min0, &Max0); 363 project(hullB,posB,ornB, sep_axis,verticesB, &Min1, &Max1); 364 365 if(Max0<Min1 || Max1<Min0) 366 return false; 367 368 float d0 = Max0 - Min1; 369 float d1 = Max1 - Min0; 370 *depth = d0<d1 ? d0:d1; 371 return true; 372} 373 374 375 376 377inline bool IsAlmostZero(const float4 v) 378{ 379 if(fabs(v.x)>1e-6f || fabs(v.y)>1e-6f || fabs(v.z)>1e-6f) 380 return false; 381 return true; 382} 383 384 385 386bool findSeparatingAxisLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, 387 const float4 posA1, 388 const float4 ornA, 389 const float4 posB1, 390 const float4 ornB, 391 const float4 DeltaC2, 392 393 const float4* verticesA, 394 const float4* uniqueEdgesA, 395 const btGpuFace* facesA, 396 const int* indicesA, 397 398 __global const float4* verticesB, 399 __global const float4* uniqueEdgesB, 400 __global const btGpuFace* facesB, 401 __global const int* indicesB, 402 float4* sep, 403 float* dmin) 404{ 405 406 407 float4 posA = posA1; 408 posA.w = 0.f; 409 float4 posB = posB1; 410 posB.w = 0.f; 411 int curPlaneTests=0; 412 { 413 int numFacesA = hullA->m_numFaces; 414 // Test normals from hullA 415 for(int i=0;i<numFacesA;i++) 416 { 417 const float4 normal = facesA[hullA->m_faceOffset+i].m_plane; 418 float4 faceANormalWS = qtRotate(ornA,normal); 419 if (dot3F4(DeltaC2,faceANormalWS)<0) 420 faceANormalWS*=-1.f; 421 curPlaneTests++; 422 float d; 423 if(!TestSepAxisLocalA( hullA, hullB, posA,ornA,posB,ornB,&faceANormalWS, verticesA, verticesB,&d)) 424 return false; 425 if(d<*dmin) 426 { 427 *dmin = d; 428 *sep = faceANormalWS; 429 } 430 } 431 } 432 if((dot3F4(-DeltaC2,*sep))>0.0f) 433 { 434 *sep = -(*sep); 435 } 436 return true; 437} 438 439bool findSeparatingAxisLocalB( __global const ConvexPolyhedronCL* hullA, const ConvexPolyhedronCL* hullB, 440 const float4 posA1, 441 const float4 ornA, 442 const float4 posB1, 443 const float4 ornB, 444 const float4 DeltaC2, 445 __global const float4* verticesA, 446 __global const float4* uniqueEdgesA, 447 __global const btGpuFace* facesA, 448 __global const int* indicesA, 449 const float4* verticesB, 450 const float4* uniqueEdgesB, 451 const btGpuFace* facesB, 452 const int* indicesB, 453 float4* sep, 454 float* dmin) 455{ 456 457 458 float4 posA = posA1; 459 posA.w = 0.f; 460 float4 posB = posB1; 461 posB.w = 0.f; 462 int curPlaneTests=0; 463 { 464 int numFacesA = hullA->m_numFaces; 465 // Test normals from hullA 466 for(int i=0;i<numFacesA;i++) 467 { 468 const float4 normal = facesA[hullA->m_faceOffset+i].m_plane; 469 float4 faceANormalWS = qtRotate(ornA,normal); 470 if (dot3F4(DeltaC2,faceANormalWS)<0) 471 faceANormalWS *= -1.f; 472 curPlaneTests++; 473 float d; 474 if(!TestSepAxisLocalA( hullB, hullA, posB,ornB,posA,ornA, &faceANormalWS, verticesB,verticesA, &d)) 475 return false; 476 if(d<*dmin) 477 { 478 *dmin = d; 479 *sep = faceANormalWS; 480 } 481 } 482 } 483 if((dot3F4(-DeltaC2,*sep))>0.0f) 484 { 485 *sep = -(*sep); 486 } 487 return true; 488} 489 490 491 492bool findSeparatingAxisEdgeEdgeLocalA( const ConvexPolyhedronCL* hullA, __global const ConvexPolyhedronCL* hullB, 493 const float4 posA1, 494 const float4 ornA, 495 const float4 posB1, 496 const float4 ornB, 497 const float4 DeltaC2, 498 const float4* verticesA, 499 const float4* uniqueEdgesA, 500 const btGpuFace* facesA, 501 const int* indicesA, 502 __global const float4* verticesB, 503 __global const float4* uniqueEdgesB, 504 __global const btGpuFace* facesB, 505 __global const int* indicesB, 506 float4* sep, 507 float* dmin) 508{ 509 510 511 float4 posA = posA1; 512 posA.w = 0.f; 513 float4 posB = posB1; 514 posB.w = 0.f; 515 516 int curPlaneTests=0; 517 518 int curEdgeEdge = 0; 519 // Test edges 520 for(int e0=0;e0<hullA->m_numUniqueEdges;e0++) 521 { 522 const float4 edge0 = uniqueEdgesA[hullA->m_uniqueEdgesOffset+e0]; 523 float4 edge0World = qtRotate(ornA,edge0); 524 525 for(int e1=0;e1<hullB->m_numUniqueEdges;e1++) 526 { 527 const float4 edge1 = uniqueEdgesB[hullB->m_uniqueEdgesOffset+e1]; 528 float4 edge1World = qtRotate(ornB,edge1); 529 530 531 float4 crossje = cross3(edge0World,edge1World); 532 533 curEdgeEdge++; 534 if(!IsAlmostZero(crossje)) 535 { 536 crossje = normalize3(crossje); 537 if (dot3F4(DeltaC2,crossje)<0) 538 crossje *= -1.f; 539 540 float dist; 541 bool result = true; 542 { 543 float Min0,Max0; 544 float Min1,Max1; 545 projectLocal(hullA,posA,ornA,&crossje,verticesA, &Min0, &Max0); 546 project(hullB,posB,ornB,&crossje,verticesB, &Min1, &Max1); 547 548 if(Max0<Min1 || Max1<Min0) 549 result = false; 550 551 float d0 = Max0 - Min1; 552 float d1 = Max1 - Min0; 553 dist = d0<d1 ? d0:d1; 554 result = true; 555 556 } 557 558 559 if(dist<*dmin) 560 { 561 *dmin = dist; 562 *sep = crossje; 563 } 564 } 565 } 566 567 } 568 569 570 if((dot3F4(-DeltaC2,*sep))>0.0f) 571 { 572 *sep = -(*sep); 573 } 574 return true; 575} 576 577 578 579inline int findClippingFaces(const float4 separatingNormal, 580 const ConvexPolyhedronCL* hullA, 581 __global const ConvexPolyhedronCL* hullB, 582 const float4 posA, const Quaternion ornA,const float4 posB, const Quaternion ornB, 583 __global float4* worldVertsA1, 584 __global float4* worldNormalsA1, 585 __global float4* worldVertsB1, 586 int capacityWorldVerts, 587 const float minDist, float maxDist, 588 const float4* verticesA, 589 const btGpuFace* facesA, 590 const int* indicesA, 591 __global const float4* verticesB, 592 __global const btGpuFace* facesB, 593 __global const int* indicesB, 594 __global int4* clippingFaces, int pairIndex) 595{ 596 int numContactsOut = 0; 597 int numWorldVertsB1= 0; 598 599 600 int closestFaceB=0; 601 float dmax = -FLT_MAX; 602 603 { 604 for(int face=0;face<hullB->m_numFaces;face++) 605 { 606 const float4 Normal = make_float4(facesB[hullB->m_faceOffset+face].m_plane.x, 607 facesB[hullB->m_faceOffset+face].m_plane.y, facesB[hullB->m_faceOffset+face].m_plane.z,0.f); 608 const float4 WorldNormal = qtRotate(ornB, Normal); 609 float d = dot3F4(WorldNormal,separatingNormal); 610 if (d > dmax) 611 { 612 dmax = d; 613 closestFaceB = face; 614 } 615 } 616 } 617 618 { 619 const btGpuFace polyB = facesB[hullB->m_faceOffset+closestFaceB]; 620 int numVertices = polyB.m_numIndices; 621 if (numVertices>capacityWorldVerts) 622 numVertices = capacityWorldVerts; 623 if (numVertices<0) 624 numVertices = 0; 625 626 for(int e0=0;e0<numVertices;e0++) 627 { 628 if (e0<capacityWorldVerts) 629 { 630 const float4 b = verticesB[hullB->m_vertexOffset+indicesB[polyB.m_indexOffset+e0]]; 631 worldVertsB1[pairIndex*capacityWorldVerts+numWorldVertsB1++] = transform(&b,&posB,&ornB); 632 } 633 } 634 } 635 636 int closestFaceA=0; 637 { 638 float dmin = FLT_MAX; 639 for(int face=0;face<hullA->m_numFaces;face++) 640 { 641 const float4 Normal = make_float4( 642 facesA[hullA->m_faceOffset+face].m_plane.x, 643 facesA[hullA->m_faceOffset+face].m_plane.y, 644 facesA[hullA->m_faceOffset+face].m_plane.z, 645 0.f); 646 const float4 faceANormalWS = qtRotate(ornA,Normal); 647 648 float d = dot3F4(faceANormalWS,separatingNormal); 649 if (d < dmin) 650 { 651 dmin = d; 652 closestFaceA = face; 653 worldNormalsA1[pairIndex] = faceANormalWS; 654 } 655 } 656 } 657 658 int numVerticesA = facesA[hullA->m_faceOffset+closestFaceA].m_numIndices; 659 if (numVerticesA>capacityWorldVerts) 660 numVerticesA = capacityWorldVerts; 661 if (numVerticesA<0) 662 numVerticesA=0; 663 664 for(int e0=0;e0<numVerticesA;e0++) 665 { 666 if (e0<capacityWorldVerts) 667 { 668 const float4 a = verticesA[hullA->m_vertexOffset+indicesA[facesA[hullA->m_faceOffset+closestFaceA].m_indexOffset+e0]]; 669 worldVertsA1[pairIndex*capacityWorldVerts+e0] = transform(&a, &posA,&ornA); 670 } 671 } 672 673 clippingFaces[pairIndex].x = closestFaceA; 674 clippingFaces[pairIndex].y = closestFaceB; 675 clippingFaces[pairIndex].z = numVerticesA; 676 clippingFaces[pairIndex].w = numWorldVertsB1; 677 678 679 return numContactsOut; 680} 681 682 683 684 685// work-in-progress 686__kernel void findConcaveSeparatingAxisVertexFaceKernel( __global int4* concavePairs, 687 __global const BodyData* rigidBodies, 688 __global const btCollidableGpu* collidables, 689 __global const ConvexPolyhedronCL* convexShapes, 690 __global const float4* vertices, 691 __global const float4* uniqueEdges, 692 __global const btGpuFace* faces, 693 __global const int* indices, 694 __global const btGpuChildShape* gpuChildShapes, 695 __global btAabbCL* aabbs, 696 __global float4* concaveSeparatingNormalsOut, 697 __global int* concaveHasSeparatingNormals, 698 __global int4* clippingFacesOut, 699 __global float4* worldVertsA1GPU, 700 __global float4* worldNormalsAGPU, 701 __global float4* worldVertsB1GPU, 702 __global float* dmins, 703 int vertexFaceCapacity, 704 int numConcavePairs 705 ) 706{ 707 708 int i = get_global_id(0); 709 if (i>=numConcavePairs) 710 return; 711 712 concaveHasSeparatingNormals[i] = 0; 713 714 int pairIdx = i; 715 716 int bodyIndexA = concavePairs[i].x; 717 int bodyIndexB = concavePairs[i].y; 718 719 int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; 720 int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; 721 722 int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; 723 int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; 724 725 if (collidables[collidableIndexB].m_shapeType!=SHAPE_CONVEX_HULL&& 726 collidables[collidableIndexB].m_shapeType!=SHAPE_COMPOUND_OF_CONVEX_HULLS) 727 { 728 concavePairs[pairIdx].w = -1; 729 return; 730 } 731 732 733 734 int numFacesA = convexShapes[shapeIndexA].m_numFaces; 735 int numActualConcaveConvexTests = 0; 736 737 int f = concavePairs[i].z; 738 739 bool overlap = false; 740 741 ConvexPolyhedronCL convexPolyhedronA; 742 743 //add 3 vertices of the triangle 744 convexPolyhedronA.m_numVertices = 3; 745 convexPolyhedronA.m_vertexOffset = 0; 746 float4 localCenter = make_float4(0.f,0.f,0.f,0.f); 747 748 btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; 749 float4 triMinAabb, triMaxAabb; 750 btAabbCL triAabb; 751 triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f); 752 triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f); 753 754 float4 verticesA[3]; 755 for (int i=0;i<3;i++) 756 { 757 int index = indices[face.m_indexOffset+i]; 758 float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; 759 verticesA[i] = vert; 760 localCenter += vert; 761 762 triAabb.m_min = min(triAabb.m_min,vert); 763 triAabb.m_max = max(triAabb.m_max,vert); 764 765 } 766 767 overlap = true; 768 overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap; 769 overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap; 770 overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap; 771 772 if (overlap) 773 { 774 float dmin = FLT_MAX; 775 int hasSeparatingAxis=5; 776 float4 sepAxis=make_float4(1,2,3,4); 777 778 int localCC=0; 779 numActualConcaveConvexTests++; 780 781 //a triangle has 3 unique edges 782 convexPolyhedronA.m_numUniqueEdges = 3; 783 convexPolyhedronA.m_uniqueEdgesOffset = 0; 784 float4 uniqueEdgesA[3]; 785 786 uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); 787 uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); 788 uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); 789 790 791 convexPolyhedronA.m_faceOffset = 0; 792 793 float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); 794 795 btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES]; 796 int indicesA[3+3+2+2+2]; 797 int curUsedIndices=0; 798 int fidx=0; 799 800 //front size of triangle 801 { 802 facesA[fidx].m_indexOffset=curUsedIndices; 803 indicesA[0] = 0; 804 indicesA[1] = 1; 805 indicesA[2] = 2; 806 curUsedIndices+=3; 807 float c = face.m_plane.w; 808 facesA[fidx].m_plane.x = normal.x; 809 facesA[fidx].m_plane.y = normal.y; 810 facesA[fidx].m_plane.z = normal.z; 811 facesA[fidx].m_plane.w = c; 812 facesA[fidx].m_numIndices=3; 813 } 814 fidx++; 815 //back size of triangle 816 { 817 facesA[fidx].m_indexOffset=curUsedIndices; 818 indicesA[3]=2; 819 indicesA[4]=1; 820 indicesA[5]=0; 821 curUsedIndices+=3; 822 float c = dot(normal,verticesA[0]); 823 float c1 = -face.m_plane.w; 824 facesA[fidx].m_plane.x = -normal.x; 825 facesA[fidx].m_plane.y = -normal.y; 826 facesA[fidx].m_plane.z = -normal.z; 827 facesA[fidx].m_plane.w = c; 828 facesA[fidx].m_numIndices=3; 829 } 830 fidx++; 831 832 bool addEdgePlanes = true; 833 if (addEdgePlanes) 834 { 835 int numVertices=3; 836 int prevVertex = numVertices-1; 837 for (int i=0;i<numVertices;i++) 838 { 839 float4 v0 = verticesA[i]; 840 float4 v1 = verticesA[prevVertex]; 841 842 float4 edgeNormal = normalize(cross(normal,v1-v0)); 843 float c = -dot(edgeNormal,v0); 844 845 facesA[fidx].m_numIndices = 2; 846 facesA[fidx].m_indexOffset=curUsedIndices; 847 indicesA[curUsedIndices++]=i; 848 indicesA[curUsedIndices++]=prevVertex; 849 850 facesA[fidx].m_plane.x = edgeNormal.x; 851 facesA[fidx].m_plane.y = edgeNormal.y; 852 facesA[fidx].m_plane.z = edgeNormal.z; 853 facesA[fidx].m_plane.w = c; 854 fidx++; 855 prevVertex = i; 856 } 857 } 858 convexPolyhedronA.m_numFaces = TRIANGLE_NUM_CONVEX_FACES; 859 convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f); 860 861 862 float4 posA = rigidBodies[bodyIndexA].m_pos; 863 posA.w = 0.f; 864 float4 posB = rigidBodies[bodyIndexB].m_pos; 865 posB.w = 0.f; 866 867 float4 ornA = rigidBodies[bodyIndexA].m_quat; 868 float4 ornB =rigidBodies[bodyIndexB].m_quat; 869 870 871 872 873 /////////////////// 874 ///compound shape support 875 876 if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) 877 { 878 int compoundChild = concavePairs[pairIdx].w; 879 int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild; 880 int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; 881 float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; 882 float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; 883 float4 newPosB = transform(&childPosB,&posB,&ornB); 884 float4 newOrnB = qtMul(ornB,childOrnB); 885 posB = newPosB; 886 ornB = newOrnB; 887 shapeIndexB = collidables[childColIndexB].m_shapeIndex; 888 } 889 ////////////////// 890 891 float4 c0local = convexPolyhedronA.m_localCenter; 892 float4 c0 = transform(&c0local, &posA, &ornA); 893 float4 c1local = convexShapes[shapeIndexB].m_localCenter; 894 float4 c1 = transform(&c1local,&posB,&ornB); 895 const float4 DeltaC2 = c0 - c1; 896 897 898 bool sepA = findSeparatingAxisLocalA( &convexPolyhedronA, &convexShapes[shapeIndexB], 899 posA,ornA, 900 posB,ornB, 901 DeltaC2, 902 verticesA,uniqueEdgesA,facesA,indicesA, 903 vertices,uniqueEdges,faces,indices, 904 &sepAxis,&dmin); 905 hasSeparatingAxis = 4; 906 if (!sepA) 907 { 908 hasSeparatingAxis = 0; 909 } else 910 { 911 bool sepB = findSeparatingAxisLocalB( &convexShapes[shapeIndexB],&convexPolyhedronA, 912 posB,ornB, 913 posA,ornA, 914 DeltaC2, 915 vertices,uniqueEdges,faces,indices, 916 verticesA,uniqueEdgesA,facesA,indicesA, 917 &sepAxis,&dmin); 918 919 if (!sepB) 920 { 921 hasSeparatingAxis = 0; 922 } else 923 { 924 hasSeparatingAxis = 1; 925 } 926 } 927 928 if (hasSeparatingAxis) 929 { 930 dmins[i] = dmin; 931 concaveSeparatingNormalsOut[pairIdx]=sepAxis; 932 concaveHasSeparatingNormals[i]=1; 933 934 } else 935 { 936 //mark this pair as in-active 937 concavePairs[pairIdx].w = -1; 938 } 939 } 940 else 941 { 942 //mark this pair as in-active 943 concavePairs[pairIdx].w = -1; 944 } 945} 946 947 948 949 950// work-in-progress 951__kernel void findConcaveSeparatingAxisEdgeEdgeKernel( __global int4* concavePairs, 952 __global const BodyData* rigidBodies, 953 __global const btCollidableGpu* collidables, 954 __global const ConvexPolyhedronCL* convexShapes, 955 __global const float4* vertices, 956 __global const float4* uniqueEdges, 957 __global const btGpuFace* faces, 958 __global const int* indices, 959 __global const btGpuChildShape* gpuChildShapes, 960 __global btAabbCL* aabbs, 961 __global float4* concaveSeparatingNormalsOut, 962 __global int* concaveHasSeparatingNormals, 963 __global int4* clippingFacesOut, 964 __global float4* worldVertsA1GPU, 965 __global float4* worldNormalsAGPU, 966 __global float4* worldVertsB1GPU, 967 __global float* dmins, 968 int vertexFaceCapacity, 969 int numConcavePairs 970 ) 971{ 972 973 int i = get_global_id(0); 974 if (i>=numConcavePairs) 975 return; 976 977 if (!concaveHasSeparatingNormals[i]) 978 return; 979 980 int pairIdx = i; 981 982 int bodyIndexA = concavePairs[i].x; 983 int bodyIndexB = concavePairs[i].y; 984 985 int collidableIndexA = rigidBodies[bodyIndexA].m_collidableIdx; 986 int collidableIndexB = rigidBodies[bodyIndexB].m_collidableIdx; 987 988 int shapeIndexA = collidables[collidableIndexA].m_shapeIndex; 989 int shapeIndexB = collidables[collidableIndexB].m_shapeIndex; 990 991 992 int numFacesA = convexShapes[shapeIndexA].m_numFaces; 993 int numActualConcaveConvexTests = 0; 994 995 int f = concavePairs[i].z; 996 997 bool overlap = false; 998 999 ConvexPolyhedronCL convexPolyhedronA; 1000 1001 //add 3 vertices of the triangle 1002 convexPolyhedronA.m_numVertices = 3; 1003 convexPolyhedronA.m_vertexOffset = 0; 1004 float4 localCenter = make_float4(0.f,0.f,0.f,0.f); 1005 1006 btGpuFace face = faces[convexShapes[shapeIndexA].m_faceOffset+f]; 1007 float4 triMinAabb, triMaxAabb; 1008 btAabbCL triAabb; 1009 triAabb.m_min = make_float4(1e30f,1e30f,1e30f,0.f); 1010 triAabb.m_max = make_float4(-1e30f,-1e30f,-1e30f,0.f); 1011 1012 float4 verticesA[3]; 1013 for (int i=0;i<3;i++) 1014 { 1015 int index = indices[face.m_indexOffset+i]; 1016 float4 vert = vertices[convexShapes[shapeIndexA].m_vertexOffset+index]; 1017 verticesA[i] = vert; 1018 localCenter += vert; 1019 1020 triAabb.m_min = min(triAabb.m_min,vert); 1021 triAabb.m_max = max(triAabb.m_max,vert); 1022 1023 } 1024 1025 overlap = true; 1026 overlap = (triAabb.m_min.x > aabbs[bodyIndexB].m_max.x || triAabb.m_max.x < aabbs[bodyIndexB].m_min.x) ? false : overlap; 1027 overlap = (triAabb.m_min.z > aabbs[bodyIndexB].m_max.z || triAabb.m_max.z < aabbs[bodyIndexB].m_min.z) ? false : overlap; 1028 overlap = (triAabb.m_min.y > aabbs[bodyIndexB].m_max.y || triAabb.m_max.y < aabbs[bodyIndexB].m_min.y) ? false : overlap; 1029 1030 if (overlap) 1031 { 1032 float dmin = dmins[i]; 1033 int hasSeparatingAxis=5; 1034 float4 sepAxis=make_float4(1,2,3,4); 1035 sepAxis = concaveSeparatingNormalsOut[pairIdx]; 1036 1037 int localCC=0; 1038 numActualConcaveConvexTests++; 1039 1040 //a triangle has 3 unique edges 1041 convexPolyhedronA.m_numUniqueEdges = 3; 1042 convexPolyhedronA.m_uniqueEdgesOffset = 0; 1043 float4 uniqueEdgesA[3]; 1044 1045 uniqueEdgesA[0] = (verticesA[1]-verticesA[0]); 1046 uniqueEdgesA[1] = (verticesA[2]-verticesA[1]); 1047 uniqueEdgesA[2] = (verticesA[0]-verticesA[2]); 1048 1049 1050 convexPolyhedronA.m_faceOffset = 0; 1051 1052 float4 normal = make_float4(face.m_plane.x,face.m_plane.y,face.m_plane.z,0.f); 1053 1054 btGpuFace facesA[TRIANGLE_NUM_CONVEX_FACES]; 1055 int indicesA[3+3+2+2+2]; 1056 int curUsedIndices=0; 1057 int fidx=0; 1058 1059 //front size of triangle 1060 { 1061 facesA[fidx].m_indexOffset=curUsedIndices; 1062 indicesA[0] = 0; 1063 indicesA[1] = 1; 1064 indicesA[2] = 2; 1065 curUsedIndices+=3; 1066 float c = face.m_plane.w; 1067 facesA[fidx].m_plane.x = normal.x; 1068 facesA[fidx].m_plane.y = normal.y; 1069 facesA[fidx].m_plane.z = normal.z; 1070 facesA[fidx].m_plane.w = c; 1071 facesA[fidx].m_numIndices=3; 1072 } 1073 fidx++; 1074 //back size of triangle 1075 { 1076 facesA[fidx].m_indexOffset=curUsedIndices; 1077 indicesA[3]=2; 1078 indicesA[4]=1; 1079 indicesA[5]=0; 1080 curUsedIndices+=3; 1081 float c = dot(normal,verticesA[0]); 1082 float c1 = -face.m_plane.w; 1083 facesA[fidx].m_plane.x = -normal.x; 1084 facesA[fidx].m_plane.y = -normal.y; 1085 facesA[fidx].m_plane.z = -normal.z; 1086 facesA[fidx].m_plane.w = c; 1087 facesA[fidx].m_numIndices=3; 1088 } 1089 fidx++; 1090 1091 bool addEdgePlanes = true; 1092 if (addEdgePlanes) 1093 { 1094 int numVertices=3; 1095 int prevVertex = numVertices-1; 1096 for (int i=0;i<numVertices;i++) 1097 { 1098 float4 v0 = verticesA[i]; 1099 float4 v1 = verticesA[prevVertex]; 1100 1101 float4 edgeNormal = normalize(cross(normal,v1-v0)); 1102 float c = -dot(edgeNormal,v0); 1103 1104 facesA[fidx].m_numIndices = 2; 1105 facesA[fidx].m_indexOffset=curUsedIndices; 1106 indicesA[curUsedIndices++]=i; 1107 indicesA[curUsedIndices++]=prevVertex; 1108 1109 facesA[fidx].m_plane.x = edgeNormal.x; 1110 facesA[fidx].m_plane.y = edgeNormal.y; 1111 facesA[fidx].m_plane.z = edgeNormal.z; 1112 facesA[fidx].m_plane.w = c; 1113 fidx++; 1114 prevVertex = i; 1115 } 1116 } 1117 convexPolyhedronA.m_numFaces = TRIANGLE_NUM_CONVEX_FACES; 1118 convexPolyhedronA.m_localCenter = localCenter*(1.f/3.f); 1119 1120 1121 float4 posA = rigidBodies[bodyIndexA].m_pos; 1122 posA.w = 0.f; 1123 float4 posB = rigidBodies[bodyIndexB].m_pos; 1124 posB.w = 0.f; 1125 1126 float4 ornA = rigidBodies[bodyIndexA].m_quat; 1127 float4 ornB =rigidBodies[bodyIndexB].m_quat; 1128 1129 1130 1131 1132 /////////////////// 1133 ///compound shape support 1134 1135 if (collidables[collidableIndexB].m_shapeType==SHAPE_COMPOUND_OF_CONVEX_HULLS) 1136 { 1137 int compoundChild = concavePairs[pairIdx].w; 1138 int childShapeIndexB = compoundChild;//collidables[collidableIndexB].m_shapeIndex+compoundChild; 1139 int childColIndexB = gpuChildShapes[childShapeIndexB].m_shapeIndex; 1140 float4 childPosB = gpuChildShapes[childShapeIndexB].m_childPosition; 1141 float4 childOrnB = gpuChildShapes[childShapeIndexB].m_childOrientation; 1142 float4 newPosB = transform(&childPosB,&posB,&ornB); 1143 float4 newOrnB = qtMul(ornB,childOrnB); 1144 posB = newPosB; 1145 ornB = newOrnB; 1146 shapeIndexB = collidables[childColIndexB].m_shapeIndex; 1147 } 1148 ////////////////// 1149 1150 float4 c0local = convexPolyhedronA.m_localCenter; 1151 float4 c0 = transform(&c0local, &posA, &ornA); 1152 float4 c1local = convexShapes[shapeIndexB].m_localCenter; 1153 float4 c1 = transform(&c1local,&posB,&ornB); 1154 const float4 DeltaC2 = c0 - c1; 1155 1156 1157 { 1158 bool sepEE = findSeparatingAxisEdgeEdgeLocalA( &convexPolyhedronA, &convexShapes[shapeIndexB], 1159 posA,ornA, 1160 posB,ornB, 1161 DeltaC2, 1162 verticesA,uniqueEdgesA,facesA,indicesA, 1163 vertices,uniqueEdges,faces,indices, 1164 &sepAxis,&dmin); 1165 1166 if (!sepEE) 1167 { 1168 hasSeparatingAxis = 0; 1169 } else 1170 { 1171 hasSeparatingAxis = 1; 1172 } 1173 } 1174 1175 1176 if (hasSeparatingAxis) 1177 { 1178 sepAxis.w = dmin; 1179 dmins[i] = dmin; 1180 concaveSeparatingNormalsOut[pairIdx]=sepAxis; 1181 concaveHasSeparatingNormals[i]=1; 1182 1183 float minDist = -1e30f; 1184 float maxDist = 0.02f; 1185 1186 1187 findClippingFaces(sepAxis, 1188 &convexPolyhedronA, 1189 &convexShapes[shapeIndexB], 1190 posA,ornA, 1191 posB,ornB, 1192 worldVertsA1GPU, 1193 worldNormalsAGPU, 1194 worldVertsB1GPU, 1195 vertexFaceCapacity, 1196 minDist, maxDist, 1197 verticesA, 1198 facesA, 1199 indicesA, 1200 vertices, 1201 faces, 1202 indices, 1203 clippingFacesOut, pairIdx); 1204 1205 1206 } else 1207 { 1208 //mark this pair as in-active 1209 concavePairs[pairIdx].w = -1; 1210 } 1211 } 1212 else 1213 { 1214 //mark this pair as in-active 1215 concavePairs[pairIdx].w = -1; 1216 } 1217 1218 concavePairs[i].z = -1;//for the next stage, z is used to determine existing contact points 1219} 1220 1221