void _cldClipBoxToCylinder(sCylinderBoxData& cData ) { dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; // check which circle from cylinder we take for clipping if ( dVector3Dot(cData.vCylinderAxis, cData.vNormal) > REAL(0.0) ) { // get top circle vCylinderCirclePos[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); vCylinderCircleNormal_Rel[0] = REAL(0.0); vCylinderCircleNormal_Rel[1] = REAL(0.0); vCylinderCircleNormal_Rel[2] = REAL(0.0); vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(-1.0); } else { // get bottom circle vCylinderCirclePos[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); vCylinderCircleNormal_Rel[0] = REAL(0.0); vCylinderCircleNormal_Rel[1] = REAL(0.0); vCylinderCircleNormal_Rel[2] = REAL(0.0); vCylinderCircleNormal_Rel[nCYLINDER_AXIS] = REAL(1.0); } // vNr is normal in Box frame, pointing from Cylinder to Box dVector3 vNr; dMatrix3 mBoxInv; // Find a way to use quaternion dMatrix3Inv(cData.mBoxRot,mBoxInv); dMultiplyMat3Vec3(mBoxInv,cData.vNormal,vNr); dVector3 vAbsNormal; vAbsNormal[0] = dFabs( vNr[0] ); vAbsNormal[1] = dFabs( vNr[1] ); vAbsNormal[2] = dFabs( vNr[2] ); // find which face in box is closest to cylinder int iB0, iB1, iB2; // Different from Croteam's code if (vAbsNormal[1] > vAbsNormal[0]) { // 1 > 0 if (vAbsNormal[0]> vAbsNormal[2]) { // 0 > 2 -> 1 > 0 >2 iB0 = 1; iB1 = 0; iB2 = 2; } else { // 2 > 0-> Must compare 1 and 2 if (vAbsNormal[1] > vAbsNormal[2]) { // 1 > 2 -> 1 > 2 > 0 iB0 = 1; iB1 = 2; iB2 = 0; } else { // 2 > 1 -> 2 > 1 > 0; iB0 = 2; iB1 = 1; iB2 = 0; } } } else { // 0 > 1 if (vAbsNormal[1] > vAbsNormal[2]) { // 1 > 2 -> 0 > 1 > 2 iB0 = 0; iB1 = 1; iB2 = 2; } else { // 2 > 1 -> Must compare 0 and 2 if (vAbsNormal[0] > vAbsNormal[2]) { // 0 > 2 -> 0 > 2 > 1; iB0 = 0; iB1 = 2; iB2 = 1; } else { // 2 > 0 -> 2 > 0 > 1; iB0 = 2; iB1 = 0; iB2 = 1; } } } dVector3 vCenter; // find center of box polygon dVector3 vTemp; if (vNr[iB0] > 0) { dMat3GetCol(cData.mBoxRot,iB0,vTemp); vCenter[0] = cData.vBoxPos[0] - cData.vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = cData.vBoxPos[1] - cData.vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = cData.vBoxPos[2] - cData.vBoxHalfSize[iB0]*vTemp[2]; } else { dMat3GetCol(cData.mBoxRot,iB0,vTemp); vCenter[0] = cData.vBoxPos[0] + cData.vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = cData.vBoxPos[1] + cData.vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = cData.vBoxPos[2] + cData.vBoxHalfSize[iB0]*vTemp[2]; } // find the vertices of box polygon dVector3 avPoints[4]; dVector3 avTempArray1[MAX_CYLBOX_CLIP_POINTS]; dVector3 avTempArray2[MAX_CYLBOX_CLIP_POINTS]; int i=0; for(i=0; i<MAX_CYLBOX_CLIP_POINTS; i++) { avTempArray1[i][0] = REAL(0.0); avTempArray1[i][1] = REAL(0.0); avTempArray1[i][2] = REAL(0.0); avTempArray2[i][0] = REAL(0.0); avTempArray2[i][1] = REAL(0.0); avTempArray2[i][2] = REAL(0.0); } dVector3 vAxis1, vAxis2; dMat3GetCol(cData.mBoxRot,iB1,vAxis1); dMat3GetCol(cData.mBoxRot,iB2,vAxis2); avPoints[0][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[0][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[0][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2]; avPoints[1][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] - cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[1][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] - cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[1][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] - cData.vBoxHalfSize[iB2] * vAxis2[2]; avPoints[2][0] = vCenter[0] - cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[2][1] = vCenter[1] - cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[2][2] = vCenter[2] - cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2]; avPoints[3][0] = vCenter[0] + cData.vBoxHalfSize[iB1] * vAxis1[0] + cData.vBoxHalfSize[iB2] * vAxis2[0]; avPoints[3][1] = vCenter[1] + cData.vBoxHalfSize[iB1] * vAxis1[1] + cData.vBoxHalfSize[iB2] * vAxis2[1]; avPoints[3][2] = vCenter[2] + cData.vBoxHalfSize[iB1] * vAxis1[2] + cData.vBoxHalfSize[iB2] * vAxis2[2]; // transform box points to space of cylinder circle dMatrix3 mCylinderInv; dMatrix3Inv(cData.mCylinderRot,mCylinderInv); for(i=0; i<4; i++) { dVector3Subtract(avPoints[i],vCylinderCirclePos,vTemp); dMultiplyMat3Vec3(mCylinderInv,vTemp,avPoints[i]); } int iTmpCounter1 = 0; int iTmpCounter2 = 0; dVector4 plPlane; // plane of cylinder that contains circle for intersection dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); dClipPolyToPlane(avPoints, 4, avTempArray1, iTmpCounter1, plPlane); // Body of base circle of Cylinder int nCircleSegment = 0; for (nCircleSegment = 0; nCircleSegment < nCYLINDER_SEGMENT; nCircleSegment++) { dConstructPlane(cData.avCylinderNormals[nCircleSegment],cData.fCylinderRadius,plPlane); if (0 == (nCircleSegment % 2)) { dClipPolyToPlane( avTempArray1 , iTmpCounter1 , avTempArray2, iTmpCounter2, plPlane); } else { dClipPolyToPlane( avTempArray2, iTmpCounter2, avTempArray1 , iTmpCounter1 , plPlane ); } dIASSERT( iTmpCounter1 >= 0 && iTmpCounter1 <= MAX_CYLBOX_CLIP_POINTS ); dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= MAX_CYLBOX_CLIP_POINTS ); } // back transform clipped points to absolute space dReal ftmpdot; dReal fTempDepth; dVector3 vPoint; if (nCircleSegment %2) { for( i=0; i<iTmpCounter2; i++) { dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray2[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, cData.vNormal); fTempDepth = cData.fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); Contact0->depth = fTempDepth; dVector3Copy(cData.vNormal,Contact0->normal); dVector3Copy(vPoint,Contact0->pos); Contact0->g1 = cData.gCylinder; Contact0->g2 = cData.gBox; dVector3Inv(Contact0->normal); cData.nContacts++; } } } else { for( i=0; i<iTmpCounter1; i++) { dMULTIPLY0_331(vPoint,cData.mCylinderRot,avTempArray1[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, cData.vNormal); fTempDepth = cData.fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(cData.iFlags, cData.gContact, cData.nContacts, cData.iSkip); Contact0->depth = fTempDepth; dVector3Copy(cData.vNormal,Contact0->normal); dVector3Copy(vPoint,Contact0->pos); Contact0->g1 = cData.gCylinder; Contact0->g2 = cData.gBox; dVector3Inv(Contact0->normal); cData.nContacts++; } } } }
// initialize collision data void _cldInitCylinderBox(sCylinderBoxData& cData) { // get cylinder position, orientation const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); dMatrix3Copy(pRotCyc,cData.mCylinderRot); const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); dVector3Copy(*pPosCyc,cData.vCylinderPos); dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); // get cylinder radius and size dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); // get box position, orientation, size const dReal* pRotBox = dGeomGetRotation(cData.gBox); dMatrix3Copy(pRotBox,cData.mBoxRot); const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(cData.gBox); dVector3Copy(*pPosBox,cData.vBoxPos); dGeomBoxGetLengths(cData.gBox, cData.vBoxHalfSize); cData.vBoxHalfSize[0] *= REAL(0.5); cData.vBoxHalfSize[1] *= REAL(0.5); cData.vBoxHalfSize[2] *= REAL(0.5); // vertex 0 cData.avBoxVertices[0][0] = -cData.vBoxHalfSize[0]; cData.avBoxVertices[0][1] = cData.vBoxHalfSize[1]; cData.avBoxVertices[0][2] = -cData.vBoxHalfSize[2]; // vertex 1 cData.avBoxVertices[1][0] = cData.vBoxHalfSize[0]; cData.avBoxVertices[1][1] = cData.vBoxHalfSize[1]; cData.avBoxVertices[1][2] = -cData.vBoxHalfSize[2]; // vertex 2 cData.avBoxVertices[2][0] = -cData.vBoxHalfSize[0]; cData.avBoxVertices[2][1] = -cData.vBoxHalfSize[1]; cData.avBoxVertices[2][2] = -cData.vBoxHalfSize[2]; // vertex 3 cData.avBoxVertices[3][0] = cData.vBoxHalfSize[0]; cData.avBoxVertices[3][1] = -cData.vBoxHalfSize[1]; cData.avBoxVertices[3][2] = -cData.vBoxHalfSize[2]; // vertex 4 cData.avBoxVertices[4][0] = cData.vBoxHalfSize[0]; cData.avBoxVertices[4][1] = cData.vBoxHalfSize[1]; cData.avBoxVertices[4][2] = cData.vBoxHalfSize[2]; // vertex 5 cData.avBoxVertices[5][0] = cData.vBoxHalfSize[0]; cData.avBoxVertices[5][1] = -cData.vBoxHalfSize[1]; cData.avBoxVertices[5][2] = cData.vBoxHalfSize[2]; // vertex 6 cData.avBoxVertices[6][0] = -cData.vBoxHalfSize[0]; cData.avBoxVertices[6][1] = -cData.vBoxHalfSize[1]; cData.avBoxVertices[6][2] = cData.vBoxHalfSize[2]; // vertex 7 cData.avBoxVertices[7][0] = -cData.vBoxHalfSize[0]; cData.avBoxVertices[7][1] = cData.vBoxHalfSize[1]; cData.avBoxVertices[7][2] = cData.vBoxHalfSize[2]; // temp index int i = 0; dVector3 vTempBoxVertices[8]; // transform vertices in absolute space for(i=0; i < 8; i++) { dMultiplyMat3Vec3(cData.mBoxRot,cData.avBoxVertices[i], vTempBoxVertices[i]); dVector3Add(vTempBoxVertices[i], cData.vBoxPos, cData.avBoxVertices[i]); } // find relative position dVector3Subtract(cData.vCylinderPos,cData.vBoxPos,cData.vDiff); cData.fBestDepth = MAX_FLOAT; cData.vNormal[0] = REAL(0.0); cData.vNormal[1] = REAL(0.0); cData.vNormal[2] = REAL(0.0); // calculate basic angle for nCYLINDER_SEGMENT-gon dReal fAngle = M_PI/nCYLINDER_SEGMENT; // calculate angle increment dReal fAngleIncrement = fAngle * REAL(2.0); // calculate nCYLINDER_SEGMENT-gon points for(i = 0; i < nCYLINDER_SEGMENT; i++) { cData.avCylinderNormals[i][0] = -dCos(fAngle); cData.avCylinderNormals[i][1] = -dSin(fAngle); cData.avCylinderNormals[i][2] = 0; fAngle += fAngleIncrement; } cData.fBestrb = 0; cData.fBestrc = 0; cData.iBestAxis = 0; cData.nContacts = 0; }
// initialize collision data void sCylinderBoxData::_cldInitCylinderBox() { // get cylinder position, orientation const dReal* pRotCyc = dGeomGetRotation(m_gCylinder); dMatrix3Copy(pRotCyc,m_mCylinderRot); const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(m_gCylinder); dVector3Copy(*pPosCyc,m_vCylinderPos); dMat3GetCol(m_mCylinderRot,nCYLINDER_AXIS,m_vCylinderAxis); // get cylinder radius and size dGeomCylinderGetParams(m_gCylinder,&m_fCylinderRadius,&m_fCylinderSize); // get box position, orientation, size const dReal* pRotBox = dGeomGetRotation(m_gBox); dMatrix3Copy(pRotBox,m_mBoxRot); const dVector3* pPosBox = (const dVector3*)dGeomGetPosition(m_gBox); dVector3Copy(*pPosBox,m_vBoxPos); dGeomBoxGetLengths(m_gBox, m_vBoxHalfSize); m_vBoxHalfSize[0] *= REAL(0.5); m_vBoxHalfSize[1] *= REAL(0.5); m_vBoxHalfSize[2] *= REAL(0.5); // vertex 0 m_avBoxVertices[0][0] = -m_vBoxHalfSize[0]; m_avBoxVertices[0][1] = m_vBoxHalfSize[1]; m_avBoxVertices[0][2] = -m_vBoxHalfSize[2]; // vertex 1 m_avBoxVertices[1][0] = m_vBoxHalfSize[0]; m_avBoxVertices[1][1] = m_vBoxHalfSize[1]; m_avBoxVertices[1][2] = -m_vBoxHalfSize[2]; // vertex 2 m_avBoxVertices[2][0] = -m_vBoxHalfSize[0]; m_avBoxVertices[2][1] = -m_vBoxHalfSize[1]; m_avBoxVertices[2][2] = -m_vBoxHalfSize[2]; // vertex 3 m_avBoxVertices[3][0] = m_vBoxHalfSize[0]; m_avBoxVertices[3][1] = -m_vBoxHalfSize[1]; m_avBoxVertices[3][2] = -m_vBoxHalfSize[2]; // vertex 4 m_avBoxVertices[4][0] = m_vBoxHalfSize[0]; m_avBoxVertices[4][1] = m_vBoxHalfSize[1]; m_avBoxVertices[4][2] = m_vBoxHalfSize[2]; // vertex 5 m_avBoxVertices[5][0] = m_vBoxHalfSize[0]; m_avBoxVertices[5][1] = -m_vBoxHalfSize[1]; m_avBoxVertices[5][2] = m_vBoxHalfSize[2]; // vertex 6 m_avBoxVertices[6][0] = -m_vBoxHalfSize[0]; m_avBoxVertices[6][1] = -m_vBoxHalfSize[1]; m_avBoxVertices[6][2] = m_vBoxHalfSize[2]; // vertex 7 m_avBoxVertices[7][0] = -m_vBoxHalfSize[0]; m_avBoxVertices[7][1] = m_vBoxHalfSize[1]; m_avBoxVertices[7][2] = m_vBoxHalfSize[2]; // temp index int i = 0; dVector3 vTempBoxVertices[8]; // transform vertices in absolute space for(i=0; i < 8; i++) { dMultiplyMat3Vec3(m_mBoxRot,m_avBoxVertices[i], vTempBoxVertices[i]); dVector3Add(vTempBoxVertices[i], m_vBoxPos, m_avBoxVertices[i]); } // find relative position dVector3Subtract(m_vCylinderPos,m_vBoxPos,m_vDiff); m_fBestDepth = MAX_FLOAT; m_vNormal[0] = REAL(0.0); m_vNormal[1] = REAL(0.0); m_vNormal[2] = REAL(0.0); // calculate basic angle for nCYLINDER_SEGMENT-gon dReal fAngle = (dReal) (M_PI/nCYLINDER_SEGMENT); // calculate angle increment dReal fAngleIncrement = fAngle * REAL(2.0); // calculate nCYLINDER_SEGMENT-gon points for(i = 0; i < nCYLINDER_SEGMENT; i++) { m_avCylinderNormals[i][0] = -dCos(fAngle); m_avCylinderNormals[i][1] = -dSin(fAngle); m_avCylinderNormals[i][2] = 0; fAngle += fAngleIncrement; } m_fBestrb = 0; m_fBestrc = 0; m_iBestAxis = 0; m_nContacts = 0; }