//---------------------------------------------------------------------------- // EgalTech 2020-2020 //---------------------------------------------------------------------------- // File : CDeRectPrismoidTria.cpp Data : 29.10.20 Versione : 2.2k1 // Contenuto : Implementazione della verifica di collisione tra // Prismoide a basi rettangolari e Triangle3d. // // // Modifiche : 29.10.20 LM Creazione modulo. // // //---------------------------------------------------------------------------- //--------------------------- Include ---------------------------------------- #include "stdafx.h" #include "CDeRectPrismoidTria.h" #include "CDeBoxTria.h" #include "CDeCylTria.h" #include "CDeSpheTria.h" #include "/EgtDev/Include/EGkIntersTriaTria.h" using namespace std ; //---------------------------------------------------------------------------- // Il sistema di riferimento deve avere l'origine nel centro della Base, // asse X lungo un lato della stessa e asse Z ortogonale alle basi e diretta verso la base Top. bool CDeSimpleRectPrismoidTria( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY, double dLenghtTopX, double dLenghtTopY, double dHeight, const Triangle3d& trTria) { // Porto il triangolo nel sistema di riferimento del tronco di cono Triangle3d trMyTria = trTria ; trMyTria.ToLoc( frPrismoid) ; double dHalfBaseX = 0.5 * dLenghtBaseX; double dHalfBaseY = 0.5 * dLenghtBaseY ; double dHalfTopX = 0.5 * dLenghtTopX ; double dHalfTopY = 0.5 * dLenghtTopY ; // Se almeno un vertice collide ho finito for ( int nV = 0 ; nV < 3 ; ++ nV) { Point3d ptVert = trMyTria.GetP( nV) ; if ( ptVert.z > - EPS_SMALL && ptVert.z < dHeight + EPS_SMALL && ( ptVert.x + dHalfBaseX + EPS_SMALL) * dHeight > ( dHalfBaseX - dHalfTopX) * ptVert.z && ( ptVert.x - dHalfBaseX - EPS_SMALL) * dHeight < ( dHalfTopX - dHalfBaseX) * ptVert.z && ( ptVert.y + dHalfBaseY + EPS_SMALL) * dHeight > ( dHalfBaseY - dHalfTopY) * ptVert.z && ( ptVert.y - dHalfBaseY - EPS_SMALL) * dHeight < ( dHalfTopY - dHalfBaseY) * ptVert.z) return true ; } // Se c'è collisione con almeno un triangolo delle facce ho finito Triangle3d trFaceTria1, trFaceTria2 ; Point3d ptInt, ptIn2 ; TRIA3DVECTOR vTria ; // Faccia base trFaceTria1.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), Point3d( - dHalfBaseX, dHalfBaseY, 0.), Point3d( dHalfBaseX, dHalfBaseY, 0.)) ; trFaceTria1.Validate() ; trFaceTria2.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), Point3d( dHalfBaseX, dHalfBaseY, 0.), Point3d( dHalfBaseX, - dHalfBaseY, 0.)) ; trFaceTria2.Validate() ; if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO || IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO) return true ; // Faccia top trFaceTria1.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight), Point3d( dHalfTopX, dHalfTopY, dHeight), Point3d( - dHalfTopX, dHalfTopY, dHeight)) ; trFaceTria1.Validate() ; trFaceTria2.Set( Point3d( - dHalfTopX, - dHalfTopY, dHeight), Point3d( dHalfTopX, - dHalfTopY, dHeight), Point3d( dHalfTopX, dHalfTopY, dHeight)) ; trFaceTria2.Validate() ; if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO || IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO) return true ; // Faccia laterale 1 trFaceTria1.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), Point3d( dHalfTopX , - dHalfTopY , dHeight), Point3d( - dHalfTopX , - dHalfTopY , dHeight)) ; trFaceTria1.Validate() ; trFaceTria2.Set( Point3d( - dHalfBaseX, - dHalfBaseY, 0.), Point3d( dHalfBaseX, - dHalfBaseY, 0.), Point3d( dHalfTopX, - dHalfTopY , dHeight)) ; trFaceTria2.Validate() ; if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO || IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO) return true ; // Faccia laterale 2 trFaceTria1.Set( Point3d( dHalfBaseX, - dHalfBaseY, 0.), Point3d( dHalfTopX , dHalfTopY , dHeight), Point3d( dHalfTopX , - dHalfTopY , dHeight)) ; trFaceTria1.Validate() ; trFaceTria2.Set( Point3d( dHalfBaseX, - dHalfBaseY, 0.), Point3d( dHalfBaseX, dHalfBaseY, 0.), Point3d( dHalfTopX , dHalfTopY , dHeight)) ; trFaceTria2.Validate() ; if ( IntersTriaTria(trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO || IntersTriaTria(trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO) return true ; // Faccia laterale 3 trFaceTria1.Set( Point3d( dHalfBaseX, dHalfBaseY, 0.), Point3d( - dHalfTopX , dHalfTopY , dHeight), Point3d( dHalfTopX , dHalfTopY , dHeight)) ; trFaceTria1.Validate() ; trFaceTria2.Set( Point3d( dHalfBaseX, dHalfBaseY, 0.), Point3d( - dHalfBaseX, dHalfBaseY, 0.), Point3d( - dHalfTopX , dHalfTopY , dHeight)) ; trFaceTria2.Validate() ; if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO || IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO) return true ; // Faccia laterale 4 trFaceTria1.Set( Point3d( - dHalfBaseX, dHalfBaseY, 0.), Point3d( - dHalfTopX , - dHalfTopY , dHeight), Point3d( - dHalfTopX , dHalfTopY , dHeight)) ; trFaceTria1.Validate() ; trFaceTria2.Set( Point3d( - dHalfBaseX, dHalfBaseY, 0.), Point3d( - dHalfBaseX, - dHalfBaseY, 0.), Point3d( - dHalfTopX , - dHalfTopY , dHeight)) ; trFaceTria2.Validate() ; if ( IntersTriaTria( trMyTria, trFaceTria1, ptInt, ptIn2, vTria) != ITTT_NO || IntersTriaTria( trMyTria, trFaceTria2, ptInt, ptIn2, vTria) != ITTT_NO) return true ; return false ; } //---------------------------------------------------------------------------- // Il sistema di riferimento deve avere l'origine nel centro della base Base, asse X lungo // un segmento della stessa e asse Z ortogonale alle basi e diretta verso la base Top. bool CDeRectPrismoidTria( const Frame3d& frPrismoid, double dLenghtBaseX, double dLenghtBaseY, double dLenghtTopX, double dLenghtTopY, double dHeight, double dSafeDist, const Triangle3d& trTria) { // Se il tronco di piramide o il triangolo non sono ben definiti non procedo if ( max( dLenghtBaseX, dLenghtTopX) < EPS_SMALL || max( dLenghtBaseY, dLenghtTopY) < EPS_SMALL || dHeight < EPS_SMALL || ! trTria.IsValid()) return false ; // Se distanza di sicurezza nulla if ( dSafeDist < EPS_SMALL) return CDeSimpleRectPrismoidTria( frPrismoid, dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, dHeight, trTria) ; // Verifiche con offset esteso double dHDiffX = ( dLenghtBaseX - dLenghtTopX) / 2 ; double dTgAx = dHDiffX / dHeight ; double dSecAx = sqrt( 1 + dTgAx * dTgAx) ; double dOffsBaseX = dSafeDist * ( dSecAx + dTgAx) ; double dOffsTopX = dSafeDist * ( dSecAx - dTgAx) ; double dHDiffY = ( dLenghtBaseY - dLenghtTopY) / 2 ; double dTgAy = dHDiffY / dHeight ; double dSecAy = sqrt( 1 + dTgAy * dTgAy) ; double dOffsBaseY = dSafeDist * ( dSecAy + dTgAy) ; double dOffsTopY = dSafeDist * ( dSecAy - dTgAy) ; Frame3d frFrame = frPrismoid ; frFrame.Translate( - dSafeDist * frFrame.VersZ()) ; if ( ! CDeSimpleRectPrismoidTria( frFrame, dLenghtBaseX + 2 * dOffsBaseX, dLenghtBaseY + 2 * dOffsBaseY, dLenghtTopX + 2 * dOffsTopX, dLenghtTopY + 2 * dOffsTopY, dHeight + 2 * dSafeDist, trTria)) return false ; // Offset fine // Box sotto frFrame = frPrismoid ; frFrame.Translate( - dSafeDist * frFrame.VersZ()) ; if ( CDeSimpleBoxTria( frFrame, Vector3d( dLenghtBaseX, dLenghtBaseY, dSafeDist), trTria)) return true ; // Box sopra frFrame = frPrismoid ; frFrame.Translate( dHeight * frFrame.VersZ()) ; if ( CDeSimpleBoxTria( frFrame, Vector3d( dLenghtTopX, dLenghtTopY, dSafeDist), trTria)) return true ; // Prismoide allungato in X double dHypoX = sqrt( dHDiffX * dHDiffX + dHeight * dHeight) ; double dOffsX = dSafeDist * dHeight / dHypoX ; double dMoveXZ = dSafeDist * dHDiffX / dHypoX ; frFrame = frPrismoid ; frFrame.Translate( dMoveXZ * frFrame.VersZ()) ; if ( CDeSimpleRectPrismoidTria( frFrame, dLenghtBaseX + 2 * dOffsX, dLenghtBaseY, dLenghtTopX + 2 * dOffsX, dLenghtTopY, dHeight, trTria)) return true ; // Prismoide allungato in Y double dHypoY = sqrt( dHDiffY * dHDiffY + dHeight * dHeight) ; double dOffsY = dSafeDist * dHeight / dHypoY ; double dMoveYZ = dSafeDist * dHDiffY / dHypoY ; frFrame = frPrismoid ; frFrame.Translate( dMoveYZ * frFrame.VersZ()) ; if ( CDeSimpleRectPrismoidTria( frFrame, dLenghtBaseX, dLenghtBaseY + 2 * dOffsY, dLenghtTopX, dLenghtTopY + 2 * dOffsY, dHeight, trTria)) return true ; // Sfere centrate nei vertici double dHalfBaseX = dLenghtBaseX / 2 ; double dHalfBaseY = dLenghtBaseY / 2 ; double dHalfTopX = dLenghtTopX / 2 ; double dHalfTopY = dLenghtTopY / 2 ; PNTVECTOR vVert = { Point3d( -dHalfBaseX, -dHalfBaseY, 0.), Point3d( dHalfBaseX, -dHalfBaseY, 0.), Point3d( dHalfBaseX, dHalfBaseY, 0.), Point3d( -dHalfBaseX, dHalfBaseY, 0.), Point3d( -dHalfTopX, -dHalfTopY, dHeight), Point3d( dHalfTopX, -dHalfTopY, dHeight), Point3d( dHalfTopX, dHalfTopY, dHeight), Point3d( -dHalfTopX, dHalfTopY, dHeight)} ; for ( auto& ptV : vVert) { ptV.ToGlob( frPrismoid) ; if ( CDeSimpleSpheTria( ptV, dSafeDist, trTria)) return true ; } // Cilindri con i segmenti come asse frFrame.Set( vVert[0], frPrismoid.VersX()) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseX, trTria)) return true ; frFrame.Set( vVert[1], frPrismoid.VersY()) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseY, trTria)) return true ; frFrame.Set( vVert[2], -frPrismoid.VersX()) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseX, trTria)) return true ; frFrame.Set( vVert[3], -frPrismoid.VersY()) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtBaseY, trTria)) return true ; frFrame.Set( vVert[4], frPrismoid.VersX()) ; if (CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopX, trTria)) return true ; frFrame.Set( vVert[5], frPrismoid.VersY()) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopY, trTria)) return true ; frFrame.Set( vVert[6], -frPrismoid.VersX()) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopX, trTria)) return true ; frFrame.Set( vVert[7], -frPrismoid.VersY()) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenghtTopY, trTria)) return true ; Vector3d vtSeg04 = vVert[4] - vVert[0] ; double dLenSeg04 = vtSeg04.Len() ; frFrame.Set( vVert[0], vtSeg04) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg04, trTria)) return true ; Vector3d vtSeg15 = vVert[5] - vVert[1] ; double dLenSeg15 = vtSeg15.Len() ; frFrame.Set( vVert[1], vtSeg15) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg15, trTria)) return true ; Vector3d vtSeg26 = vVert[6] - vVert[2] ; double dLenSeg26 = vtSeg26.Len() ; frFrame.Set( vVert[2], vtSeg26) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg26, trTria)) return true ; Vector3d vtSeg37 = vVert[7] - vVert[3] ; double dLenSeg37 = vtSeg37.Len(); frFrame.Set( vVert[3], vtSeg37) ; if ( CDeSimpleCylTria( frFrame, dSafeDist, dLenSeg37, trTria)) return true ; return false ; }