int _cldClipCylinderToBox(sCylinderBoxData& cData) { // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal dVector3 vN; dReal fTemp1 = dVector3Dot(cData.vCylinderAxis,cData.vNormal); vN[0] = cData.vNormal[0] - cData.vCylinderAxis[0]*fTemp1; vN[1] = cData.vNormal[1] - cData.vCylinderAxis[1]*fTemp1; vN[2] = cData.vNormal[2] - cData.vCylinderAxis[2]*fTemp1; // normalize that vector dNormalize3(vN); // translate cylinder end points by the vector dVector3 vCposTrans; vCposTrans[0] = cData.vCylinderPos[0] + vN[0] * cData.fCylinderRadius; vCposTrans[1] = cData.vCylinderPos[1] + vN[1] * cData.fCylinderRadius; vCposTrans[2] = cData.vCylinderPos[2] + vN[2] * cData.fCylinderRadius; cData.vEp0[0] = vCposTrans[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); cData.vEp0[1] = vCposTrans[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); cData.vEp0[2] = vCposTrans[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); cData.vEp1[0] = vCposTrans[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); cData.vEp1[1] = vCposTrans[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); cData.vEp1[2] = vCposTrans[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); // transform edge points in box space cData.vEp0[0] -= cData.vBoxPos[0]; cData.vEp0[1] -= cData.vBoxPos[1]; cData.vEp0[2] -= cData.vBoxPos[2]; cData.vEp1[0] -= cData.vBoxPos[0]; cData.vEp1[1] -= cData.vBoxPos[1]; cData.vEp1[2] -= cData.vBoxPos[2]; dVector3 vTemp1; // clip the edge to box dVector4 plPlane; // plane 0 +x dMat3GetCol(cData.mBoxRot,0,vTemp1); dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) { return 0; } // plane 1 +y dMat3GetCol(cData.mBoxRot,1,vTemp1); dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) { return 0; } // plane 2 +z dMat3GetCol(cData.mBoxRot,2,vTemp1); dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) { return 0; } // plane 3 -x dMat3GetCol(cData.mBoxRot,0,vTemp1); dVector3Inv(vTemp1); dConstructPlane(vTemp1,cData.vBoxHalfSize[0],plPlane); if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) { return 0; } // plane 4 -y dMat3GetCol(cData.mBoxRot,1,vTemp1); dVector3Inv(vTemp1); dConstructPlane(vTemp1,cData.vBoxHalfSize[1],plPlane); if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) { return 0; } // plane 5 -z dMat3GetCol(cData.mBoxRot,2,vTemp1); dVector3Inv(vTemp1); dConstructPlane(vTemp1,cData.vBoxHalfSize[2],plPlane); if(!dClipEdgeToPlane( cData.vEp0, cData.vEp1, plPlane )) { return 0; } // calculate depths for both contact points cData.fDepth0 = cData.fBestrb + dVector3Dot(cData.vEp0, cData.vNormal); cData.fDepth1 = cData.fBestrb + dVector3Dot(cData.vEp1, cData.vNormal); // clamp depths to 0 if(cData.fDepth0<0) { cData.fDepth0 = REAL(0.0); } if(cData.fDepth1<0) { cData.fDepth1 = REAL(0.0); } // back transform edge points from box to absolute space cData.vEp0[0] += cData.vBoxPos[0]; cData.vEp0[1] += cData.vBoxPos[1]; cData.vEp0[2] += cData.vBoxPos[2]; cData.vEp1[0] += cData.vBoxPos[0]; cData.vEp1[1] += cData.vBoxPos[1]; cData.vEp1[2] += cData.vBoxPos[2]; dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); Contact0->depth = cData.fDepth0; dVector3Copy(cData.vNormal,Contact0->normal); dVector3Copy(cData.vEp0,Contact0->pos); Contact0->g1 = cData.gCylinder; Contact0->g2 = cData.gBox; dVector3Inv(Contact0->normal); cData.nContacts++; dContactGeom* Contact1 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); Contact1->depth = cData.fDepth1; dVector3Copy(cData.vNormal,Contact1->normal); dVector3Copy(cData.vEp1,Contact1->pos); Contact1->g1 = cData.gCylinder; Contact1->g2 = cData.gBox; dVector3Inv(Contact1->normal); cData.nContacts++; return 1; }
bool _cldClipCylinderEdgeToTriangle(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { // translate cylinder dReal fTemp = dVector3Dot(cData.vCylinderAxis , cData.vContactNormal); dVector3 vN2; vN2[0] = cData.vContactNormal[0] - cData.vCylinderAxis[0]*fTemp; vN2[1] = cData.vContactNormal[1] - cData.vCylinderAxis[1]*fTemp; vN2[2] = cData.vContactNormal[2] - cData.vCylinderAxis[2]*fTemp; fTemp = dVector3Length(vN2); if (fTemp < REAL(1e-5)) { return false; } // Normalize it vN2[0] /= fTemp; vN2[1] /= fTemp; vN2[2] /= fTemp; // calculate caps centers in absolute space dVector3 vCposTrans; vCposTrans[0] = cData.vCylinderPos[0] + vN2[0]*cData.fCylinderRadius; vCposTrans[1] = cData.vCylinderPos[1] + vN2[1]*cData.fCylinderRadius; vCposTrans[2] = cData.vCylinderPos[2] + vN2[2]*cData.fCylinderRadius; dVector3 vCEdgePoint0; vCEdgePoint0[0] = vCposTrans[0] + cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); vCEdgePoint0[1] = vCposTrans[1] + cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); vCEdgePoint0[2] = vCposTrans[2] + cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5)); dVector3 vCEdgePoint1; vCEdgePoint1[0] = vCposTrans[0] - cData.vCylinderAxis[0] * (cData.fCylinderSize* REAL(0.5)); vCEdgePoint1[1] = vCposTrans[1] - cData.vCylinderAxis[1] * (cData.fCylinderSize* REAL(0.5)); vCEdgePoint1[2] = vCposTrans[2] - cData.vCylinderAxis[2] * (cData.fCylinderSize* REAL(0.5)); // transform cylinder edge points into triangle space vCEdgePoint0[0] -= v0[0]; vCEdgePoint0[1] -= v0[1]; vCEdgePoint0[2] -= v0[2]; vCEdgePoint1[0] -= v0[0]; vCEdgePoint1[1] -= v0[1]; vCEdgePoint1[2] -= v0[2]; dVector4 plPlane; dVector3 vPlaneNormal; // triangle plane //plPlane = Plane4f( -cData.vNormal, 0); vPlaneNormal[0] = -cData.vNormal[0]; vPlaneNormal[1] = -cData.vNormal[1]; vPlaneNormal[2] = -cData.vNormal[2]; dConstructPlane(vPlaneNormal,REAL(0.0),plPlane); if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return false; } // plane with edge 0 //plPlane = Plane4f( ( cData.vNormal cross cData.vE0 ), REAL(1e-5)); dVector3Cross(cData.vNormal,cData.vE0,vPlaneNormal); dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return false; } // plane with edge 1 //dVector3 vTemp = ( cData.vNormal cross cData.vE1 ); dVector3Cross(cData.vNormal,cData.vE1,vPlaneNormal); fTemp = dVector3Dot(cData.vE0 , vPlaneNormal) - REAL(1e-5); //plPlane = Plane4f( vTemp, -(( cData.vE0 dot vTemp )-REAL(1e-5))); dConstructPlane(vPlaneNormal,-fTemp,plPlane); if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return false; } // plane with edge 2 // plPlane = Plane4f( ( cData.vNormal cross cData.vE2 ), REAL(1e-5)); dVector3Cross(cData.vNormal,cData.vE2,vPlaneNormal); dConstructPlane(vPlaneNormal,REAL(1e-5),plPlane); if(!dClipEdgeToPlane( vCEdgePoint0, vCEdgePoint1, plPlane )) { return false; } // return capsule edge points into absolute space vCEdgePoint0[0] += v0[0]; vCEdgePoint0[1] += v0[1]; vCEdgePoint0[2] += v0[2]; vCEdgePoint1[0] += v0[0]; vCEdgePoint1[1] += v0[1]; vCEdgePoint1[2] += v0[2]; // calculate depths for both contact points dVector3 vTemp; dVector3Subtract(vCEdgePoint0,cData.vCylinderPos, vTemp); dReal fRestDepth0 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; dVector3Subtract(vCEdgePoint1,cData.vCylinderPos, vTemp); dReal fRestDepth1 = -dVector3Dot(vTemp,cData.vContactNormal) + cData.fBestrt; dReal fDepth0 = cData.fBestDepth - (fRestDepth0); dReal fDepth1 = cData.fBestDepth - (fRestDepth1); // clamp depths to zero if(fDepth0 < REAL(0.0) ) { fDepth0 = REAL(0.0); } if(fDepth1<REAL(0.0)) { fDepth1 = REAL(0.0); } // Generate contact 0 { cData.gLocalContacts[cData.nContacts].fDepth = fDepth0; dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); dVector3Copy(vCEdgePoint0,cData.gLocalContacts[cData.nContacts].vPos); cData.gLocalContacts[cData.nContacts].nFlags = 1; cData.nContacts++; if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) return true; } // Generate contact 1 { // generate contacts cData.gLocalContacts[cData.nContacts].fDepth = fDepth1; dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); dVector3Copy(vCEdgePoint1,cData.gLocalContacts[cData.nContacts].vPos); cData.gLocalContacts[cData.nContacts].nFlags = 1; cData.nContacts++; } return true; }
int sCylinderBoxData::_cldClipCylinderToBox() { dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK)); // calculate that vector perpendicular to cylinder axis which closes lowest angle with collision normal dVector3 vN; dReal fTemp1 = dVector3Dot(m_vCylinderAxis,m_vNormal); vN[0] = m_vNormal[0] - m_vCylinderAxis[0]*fTemp1; vN[1] = m_vNormal[1] - m_vCylinderAxis[1]*fTemp1; vN[2] = m_vNormal[2] - m_vCylinderAxis[2]*fTemp1; // normalize that vector dNormalize3(vN); // translate cylinder end points by the vector dVector3 vCposTrans; vCposTrans[0] = m_vCylinderPos[0] + vN[0] * m_fCylinderRadius; vCposTrans[1] = m_vCylinderPos[1] + vN[1] * m_fCylinderRadius; vCposTrans[2] = m_vCylinderPos[2] + vN[2] * m_fCylinderRadius; m_vEp0[0] = vCposTrans[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); m_vEp0[1] = vCposTrans[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); m_vEp0[2] = vCposTrans[2] + m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); m_vEp1[0] = vCposTrans[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); m_vEp1[1] = vCposTrans[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); m_vEp1[2] = vCposTrans[2] - m_vCylinderAxis[2]*(m_fCylinderSize*REAL(0.5)); // transform edge points in box space m_vEp0[0] -= m_vBoxPos[0]; m_vEp0[1] -= m_vBoxPos[1]; m_vEp0[2] -= m_vBoxPos[2]; m_vEp1[0] -= m_vBoxPos[0]; m_vEp1[1] -= m_vBoxPos[1]; m_vEp1[2] -= m_vBoxPos[2]; dVector3 vTemp1; // clip the edge to box dVector4 plPlane; // plane 0 +x dMat3GetCol(m_mBoxRot,0,vTemp1); dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane); if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) { return 0; } // plane 1 +y dMat3GetCol(m_mBoxRot,1,vTemp1); dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane); if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) { return 0; } // plane 2 +z dMat3GetCol(m_mBoxRot,2,vTemp1); dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane); if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) { return 0; } // plane 3 -x dMat3GetCol(m_mBoxRot,0,vTemp1); dVector3Inv(vTemp1); dConstructPlane(vTemp1,m_vBoxHalfSize[0],plPlane); if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) { return 0; } // plane 4 -y dMat3GetCol(m_mBoxRot,1,vTemp1); dVector3Inv(vTemp1); dConstructPlane(vTemp1,m_vBoxHalfSize[1],plPlane); if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) { return 0; } // plane 5 -z dMat3GetCol(m_mBoxRot,2,vTemp1); dVector3Inv(vTemp1); dConstructPlane(vTemp1,m_vBoxHalfSize[2],plPlane); if(!dClipEdgeToPlane( m_vEp0, m_vEp1, plPlane )) { return 0; } // calculate depths for both contact points m_fDepth0 = m_fBestrb + dVector3Dot(m_vEp0, m_vNormal); m_fDepth1 = m_fBestrb + dVector3Dot(m_vEp1, m_vNormal); // clamp depths to 0 if(m_fDepth0<0) { m_fDepth0 = REAL(0.0); } if(m_fDepth1<0) { m_fDepth1 = REAL(0.0); } // back transform edge points from box to absolute space m_vEp0[0] += m_vBoxPos[0]; m_vEp0[1] += m_vBoxPos[1]; m_vEp0[2] += m_vBoxPos[2]; m_vEp1[0] += m_vBoxPos[0]; m_vEp1[1] += m_vBoxPos[1]; m_vEp1[2] += m_vBoxPos[2]; dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); Contact0->depth = m_fDepth0; dVector3Copy(m_vNormal,Contact0->normal); dVector3Copy(m_vEp0,Contact0->pos); Contact0->g1 = m_gCylinder; Contact0->g2 = m_gBox; Contact0->side1 = -1; Contact0->side2 = -1; dVector3Inv(Contact0->normal); m_nContacts++; if (m_nContacts != (m_iFlags & NUMC_MASK)) { dContactGeom* Contact1 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); Contact1->depth = m_fDepth1; dVector3Copy(m_vNormal,Contact1->normal); dVector3Copy(m_vEp1,Contact1->pos); Contact1->g1 = m_gCylinder; Contact1->g2 = m_gBox; Contact1->side1 = -1; Contact1->side2 = -1; dVector3Inv(Contact1->normal); m_nContacts++; } return 1; }