// clip and generate contacts static void _cldClipping(const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { // if we have edge/edge intersection if ( iBestAxis > 4 ) { dVector3 vub,vPb,vPa; SET(vPa,vHullBoxPos); // calculate point on box edge for( int i=0; i<3; i++) { dVector3 vRotCol; GETCOL(mHullBoxRot,i,vRotCol); dReal fSign = dDOT(vBestNormal,vRotCol) > 0 ? 1.0f : -1.0f; vPa[0] += fSign * vBoxHalfSize[i] * vRotCol[0]; vPa[1] += fSign * vBoxHalfSize[i] * vRotCol[1]; vPa[2] += fSign * vBoxHalfSize[i] * vRotCol[2]; } int iEdge = (iBestAxis-5)%3; // decide which edge is on triangle if ( iEdge == 0 ) { SET(vPb,v0); SET(vub,vE0); } else if ( iEdge == 1) { SET(vPb,v2); SET(vub,vE1); } else { SET(vPb,v1); SET(vub,vE2); } // setup direction parameter for face edge dNormalize3(vub); dReal fParam1, fParam2; // setup direction parameter for box edge dVector3 vua; int col=(iBestAxis-5)/3; GETCOL(mHullBoxRot,col,vua); // find two closest points on both edges _cldClosestPointOnTwoLines( vPa, vua, vPb, vub, fParam1, fParam2 ); vPa[0] += vua[0]*fParam1; vPa[1] += vua[1]*fParam1; vPa[2] += vua[2]*fParam1; vPb[0] += vub[0]*fParam2; vPb[1] += vub[1]*fParam2; vPb[2] += vub[2]*fParam2; // calculate collision point dVector3 vPntTmp; ADD(vPa,vPb,vPntTmp); vPntTmp[0]*=0.5f; vPntTmp[1]*=0.5f; vPntTmp[2]*=0.5f; // generate contact point between two closest points #ifdef ORIG if (ctContacts < (iFlags & 0x0ffff)) { dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride); Contact->depth = fBestDepth; SET(Contact->normal,vBestNormal); SET(Contact->pos,vPntTmp); Contact->g1 = Geom1; Contact->g2 = Geom2; ctContacts++; } #endif GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2, vPntTmp, vBestNormal, fBestDepth, ctContacts); // if triangle is the referent face then clip box to triangle face } else if ( iBestAxis == 1 ) { dVector3 vNormal2; vNormal2[0]=-vBestNormal[0]; vNormal2[1]=-vBestNormal[1]; vNormal2[2]=-vBestNormal[2]; // vNr is normal in box frame, pointing from triangle to box dMatrix3 mTransposed; mTransposed[0*4+0]=mHullBoxRot[0*4+0]; mTransposed[0*4+1]=mHullBoxRot[1*4+0]; mTransposed[0*4+2]=mHullBoxRot[2*4+0]; mTransposed[1*4+0]=mHullBoxRot[0*4+1]; mTransposed[1*4+1]=mHullBoxRot[1*4+1]; mTransposed[1*4+2]=mHullBoxRot[2*4+1]; mTransposed[2*4+0]=mHullBoxRot[0*4+2]; mTransposed[2*4+1]=mHullBoxRot[1*4+2]; mTransposed[2*4+2]=mHullBoxRot[2*4+2]; dVector3 vNr; vNr[0]=mTransposed[0*4+0]*vNormal2[0]+ mTransposed[0*4+1]*vNormal2[1]+ mTransposed[0*4+2]*vNormal2[2]; vNr[1]=mTransposed[1*4+0]*vNormal2[0]+ mTransposed[1*4+1]*vNormal2[1]+ mTransposed[1*4+2]*vNormal2[2]; vNr[2]=mTransposed[2*4+0]*vNormal2[0]+ mTransposed[2*4+1]*vNormal2[1]+ mTransposed[2*4+2]*vNormal2[2]; dVector3 vAbsNormal; vAbsNormal[0] = dFabs( vNr[0] ); vAbsNormal[1] = dFabs( vNr[1] ); vAbsNormal[2] = dFabs( vNr[2] ); // get closest face from box int iB0, iB1, iB2; if (vAbsNormal[1] > vAbsNormal[0]) { if (vAbsNormal[1] > vAbsNormal[2]) { iB1 = 0; iB0 = 1; iB2 = 2; } else { iB1 = 0; iB2 = 1; iB0 = 2; } } else { if (vAbsNormal[0] > vAbsNormal[2]) { iB0 = 0; iB1 = 1; iB2 = 2; } else { iB1 = 0; iB2 = 1; iB0 = 2; } } // Here find center of box face we are going to project dVector3 vCenter; dVector3 vRotCol; GETCOL(mHullBoxRot,iB0,vRotCol); if (vNr[iB0] > 0) { vCenter[0] = vHullBoxPos[0] - v0[0] - vBoxHalfSize[iB0] * vRotCol[0]; vCenter[1] = vHullBoxPos[1] - v0[1] - vBoxHalfSize[iB0] * vRotCol[1]; vCenter[2] = vHullBoxPos[2] - v0[2] - vBoxHalfSize[iB0] * vRotCol[2]; } else { vCenter[0] = vHullBoxPos[0] - v0[0] + vBoxHalfSize[iB0] * vRotCol[0]; vCenter[1] = vHullBoxPos[1] - v0[1] + vBoxHalfSize[iB0] * vRotCol[1]; vCenter[2] = vHullBoxPos[2] - v0[2] + vBoxHalfSize[iB0] * vRotCol[2]; } // Here find 4 corner points of box dVector3 avPoints[4]; dVector3 vRotCol2; GETCOL(mHullBoxRot,iB1,vRotCol); GETCOL(mHullBoxRot,iB2,vRotCol2); for(int x=0;x<3;x++) { avPoints[0][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[1][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) - (vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[2][x] = vCenter[x] - (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]); avPoints[3][x] = vCenter[x] + (vBoxHalfSize[iB1] * vRotCol[x]) + (vBoxHalfSize[iB2] * vRotCol2[x]); } // clip Box face with 4 planes of triangle (1 face plane, 3 egde planes) dVector3 avTempArray1[9]; dVector3 avTempArray2[9]; dVector4 plPlane; int iTempCnt1=0; int iTempCnt2=0; // zeroify vectors - necessary? for(int i=0; i<9; i++) { avTempArray1[i][0]=0; avTempArray1[i][1]=0; avTempArray1[i][2]=0; avTempArray2[i][0]=0; avTempArray2[i][1]=0; avTempArray2[i][2]=0; } // Normal plane dVector3 vTemp; vTemp[0]=-vN[0]; vTemp[1]=-vN[1]; vTemp[2]=-vN[2]; dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avPoints, 4, avTempArray1, iTempCnt1, plPlane ); // Plane p0 dVector3 vTemp2; SUBTRACT(v1,v0,vTemp2); dCROSS(vTemp,=,vN,vTemp2); dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // Plane p1 SUBTRACT(v2,v1,vTemp2); dCROSS(vTemp,=,vN,vTemp2); dNormalize3(vTemp); SUBTRACT(v0,v2,vTemp2); CONSTRUCTPLANE(plPlane,vTemp,dDOT(vTemp2,vTemp)); _cldClipPolyToPlane( avTempArray2, iTempCnt2, avTempArray1, iTempCnt1, plPlane ); // Plane p2 SUBTRACT(v0,v2,vTemp2); dCROSS(vTemp,=,vN,vTemp2); dNormalize3(vTemp); CONSTRUCTPLANE(plPlane,vTemp,0); _cldClipPolyToPlane( avTempArray1, iTempCnt1, avTempArray2, iTempCnt2, plPlane ); // END of clipping polygons // for each generated contact point for ( int i=0; i<iTempCnt2; i++ ) { // calculate depth dReal fTempDepth = dDOT(vNormal2,avTempArray2[i]); // clamp depth to zero if (fTempDepth > 0) { fTempDepth = 0; } dVector3 vPntTmp; ADD(avTempArray2[i],v0,vPntTmp); #ifdef ORIG if (ctContacts < (iFlags & 0x0ffff)) { dContactGeom* Contact = SAFECONTACT(iFlags, ContactGeoms, ctContacts, iStride); Contact->depth = -fTempDepth; SET(Contact->normal,vBestNormal); SET(Contact->pos,vPntTmp); Contact->g1 = Geom1; Contact->g2 = Geom2; ctContacts++; } #endif GenerateContact(iFlags, ContactGeoms, iStride, Geom1, Geom2, vPntTmp, vBestNormal, -fTempDepth, ctContacts); } //dAASSERT(ctContacts>0); // if box face is the referent face, then clip triangle on box face } else { // 2 <= if iBestAxis <= 4
// test one mesh triangle on intersection with capsule void sTrimeshCapsuleColliderData::_cldTestOneTriangleVSCapsule( const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, uint8 flags) { // calculate edges SUBTRACT(v1,v0,m_vE0); SUBTRACT(v2,v1,m_vE1); SUBTRACT(v0,v2,m_vE2); dVector3 _minus_vE0; SUBTRACT(v0,v1,_minus_vE0); // calculate poly normal dCalcVectorCross3(m_vN,m_vE1,_minus_vE0); // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying // space transformation. if (!dSafeNormalize3(m_vN)) { return; } // create plane from triangle dReal plDistance = -dCalcVectorDot3(v0,m_vN); dVector4 plTrianglePlane; CONSTRUCTPLANE(plTrianglePlane,m_vN,plDistance); // calculate capsule distance to plane dReal fDistanceCapsuleCenterToPlane = POINTDISTANCE(plTrianglePlane,m_vCapsulePosition); // Capsule must be over positive side of triangle if (fDistanceCapsuleCenterToPlane < 0 /* && !bDoubleSided*/) { // if not don't generate contacts return; } dVector3 vPnt0; SET (vPnt0,v0); dVector3 vPnt1; SET (vPnt1,v1); dVector3 vPnt2; SET (vPnt2,v2); if (fDistanceCapsuleCenterToPlane < 0 ) { SET (vPnt0,v0); SET (vPnt1,v2); SET (vPnt2,v1); } // do intersection test and find best separating axis if (!_cldTestSeparatingAxesOfCapsule(vPnt0, vPnt1, vPnt2, flags)) { // if not found do nothing return; } // if best separation axis is not found if (m_iBestAxis == 0 ) { // this should not happen (we should already exit in that case) dIASSERT(FALSE); // do nothing return; } // calculate caps centers in absolute space dVector3 vCposTrans; vCposTrans[0] = m_vCapsulePosition[0] + m_vNormal[0]*m_vCapsuleRadius; vCposTrans[1] = m_vCapsulePosition[1] + m_vNormal[1]*m_vCapsuleRadius; vCposTrans[2] = m_vCapsulePosition[2] + m_vNormal[2]*m_vCapsuleRadius; dVector3 vCEdgePoint0; vCEdgePoint0[0] = vCposTrans[0] + m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); vCEdgePoint0[1] = vCposTrans[1] + m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); vCEdgePoint0[2] = vCposTrans[2] + m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); dVector3 vCEdgePoint1; vCEdgePoint1[0] = vCposTrans[0] - m_vCapsuleAxis[0]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); vCEdgePoint1[1] = vCposTrans[1] - m_vCapsuleAxis[1]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); vCEdgePoint1[2] = vCposTrans[2] - m_vCapsuleAxis[2]*(m_fCapsuleSize*REAL(0.5)-m_vCapsuleRadius); // transform capsule edge points into triangle space vCEdgePoint0[0] -= vPnt0[0]; vCEdgePoint0[1] -= vPnt0[1]; vCEdgePoint0[2] -= vPnt0[2]; vCEdgePoint1[0] -= vPnt0[0]; vCEdgePoint1[1] -= vPnt0[1]; vCEdgePoint1[2] -= vPnt0[2]; dVector4 plPlane; dVector3 _minus_vN; _minus_vN[0] = -m_vN[0]; _minus_vN[1] = -m_vN[1]; _minus_vN[2] = -m_vN[2]; // triangle plane CONSTRUCTPLANE(plPlane,_minus_vN,0); //plPlane = Plane4f( -m_vN, 0); if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return; } // plane with edge 0 dVector3 vTemp; dCalcVectorCross3(vTemp,m_vN,m_vE0); CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return; } dCalcVectorCross3(vTemp,m_vN,m_vE1); CONSTRUCTPLANE(plPlane, vTemp, -(dCalcVectorDot3(m_vE0,vTemp)-REAL(1e-5))); if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return; } dCalcVectorCross3(vTemp,m_vN,m_vE2); CONSTRUCTPLANE(plPlane, vTemp, REAL(1e-5)); if (!_cldClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return; } // return capsule edge points into absolute space vCEdgePoint0[0] += vPnt0[0]; vCEdgePoint0[1] += vPnt0[1]; vCEdgePoint0[2] += vPnt0[2]; vCEdgePoint1[0] += vPnt0[0]; vCEdgePoint1[1] += vPnt0[1]; vCEdgePoint1[2] += vPnt0[2]; // calculate depths for both contact points SUBTRACT(vCEdgePoint0,m_vCapsulePosition,vTemp); dReal fDepth0 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt); SUBTRACT(vCEdgePoint1,m_vCapsulePosition,vTemp); dReal fDepth1 = dCalcVectorDot3(vTemp,m_vNormal) - (m_fBestCenter-m_fBestrt); // clamp depths to zero if (fDepth0 < 0) { fDepth0 = 0.0f; } if (fDepth1 < 0 ) { fDepth1 = 0.0f; } // Cached contacts's data // contact 0 dIASSERT(m_ctContacts < (m_iFlags & NUMC_MASK)); // Do not call function if there is no room to store result m_gLocalContacts[m_ctContacts].fDepth = fDepth0; SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal); SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint0); m_gLocalContacts[m_ctContacts].nFlags = 1; m_ctContacts++; if (m_ctContacts < (m_iFlags & NUMC_MASK)) { // contact 1 m_gLocalContacts[m_ctContacts].fDepth = fDepth1; SET(m_gLocalContacts[m_ctContacts].vNormal,m_vNormal); SET(m_gLocalContacts[m_ctContacts].vPos,vCEdgePoint1); m_gLocalContacts[m_ctContacts].nFlags = 1; m_ctContacts++; } }