//---------------------------------------------------------------------------- // 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 "CDeCapsTria.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, const Triangle3d& trTria, double dSafeDist) { // 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 ; // Porto il triangolo nel riferimento del prismoide Triangle3d trTriaL = trTria ; trTriaL.ToLoc( frPrismoid) ; // Se distanza di sicurezza nulla if ( dSafeDist < EPS_SMALL) return CDeSimpleRectPrismoidTria( Frame3d(), dLenghtBaseX, dLenghtBaseY, dLenghtTopX, dLenghtTopY, dHeight, trTriaL) ; // 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 frTmp( Point3d( 0, 0, -dSafeDist)) ; if ( ! CDeSimpleRectPrismoidTria( frTmp, dLenghtBaseX + 2 * dOffsBaseX, dLenghtBaseY + 2 * dOffsBaseY, dLenghtTopX + 2 * dOffsTopX, dLenghtTopY + 2 * dOffsTopY, dHeight + 2 * dSafeDist, trTriaL)) return false ; // Offset fine // Box sotto frTmp.ChangeOrig( Point3d( 0, 0, -dSafeDist)) ; if ( CDeSimpleBoxTria( frTmp, Vector3d( dLenghtBaseX, dLenghtBaseY, dSafeDist), trTriaL)) return true ; // Box sopra frTmp.ChangeOrig( Point3d( 0, 0, dHeight)) ; if ( CDeSimpleBoxTria( frTmp, Vector3d( dLenghtTopX, dLenghtTopY, dSafeDist), trTriaL)) return true ; // Prismoide allungato in X double dHypoX = sqrt( dHDiffX * dHDiffX + dHeight * dHeight) ; double dOffsX = dSafeDist * dHeight / dHypoX ; double dMoveXZ = dSafeDist * dHDiffX / dHypoX ; frTmp.ChangeOrig( Point3d( 0, 0, dMoveXZ)) ; if ( CDeSimpleRectPrismoidTria( frTmp, dLenghtBaseX + 2 * dOffsX, dLenghtBaseY, dLenghtTopX + 2 * dOffsX, dLenghtTopY, dHeight, trTriaL)) return true ; // Prismoide allungato in Y double dHypoY = sqrt( dHDiffY * dHDiffY + dHeight * dHeight) ; double dOffsY = dSafeDist * dHeight / dHypoY ; double dMoveYZ = dSafeDist * dHDiffY / dHypoY ; frTmp.ChangeOrig( Point3d( 0, 0, dMoveYZ)) ; if ( CDeSimpleRectPrismoidTria( frTmp, dLenghtBaseX, dLenghtBaseY + 2 * dOffsY, dLenghtTopX, dLenghtTopY + 2 * dOffsY, dHeight, trTriaL)) return true ; // 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)} ; // Capsule centrati sui dodici spigoli if ( CDeSimpleCapsTria( vVert[0], vVert[1], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[1], vVert[2], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[2], vVert[3], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[3], vVert[0], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[4], vVert[5], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[5], vVert[6], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[6], vVert[7], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[7], vVert[4], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[0], vVert[4], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[1], vVert[5], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[2], vVert[6], dSafeDist, trTriaL)) return true ; if ( CDeSimpleCapsTria( vVert[3], vVert[7], dSafeDist, trTriaL)) return true ; return false ; }