void _InitCylinderTrimeshData(sData& cData) { // get cylinder information // Rotation const dReal* pRotCyc = dGeomGetRotation(cData.gCylinder); dMatrix3Copy(pRotCyc,cData.mCylinderRot); dGeomGetQuaternion(cData.gCylinder,cData.qCylinderRot); // Position const dVector3* pPosCyc = (const dVector3*)dGeomGetPosition(cData.gCylinder); dVector3Copy(*pPosCyc,cData.vCylinderPos); // Cylinder axis dMat3GetCol(cData.mCylinderRot,nCYLINDER_AXIS,cData.vCylinderAxis); // get cylinder radius and size dGeomCylinderGetParams(cData.gCylinder,&cData.fCylinderRadius,&cData.fCylinderSize); // get trimesh position and orientation const dReal* pRotTris = dGeomGetRotation(cData.gTrimesh); dMatrix3Copy(pRotTris,cData.mTrimeshRot); dGeomGetQuaternion(cData.gTrimesh,cData.qTrimeshRot); // Position const dVector3* pPosTris = (const dVector3*)dGeomGetPosition(cData.gTrimesh); dVector3Copy(*pPosTris,cData.vTrimeshPos); // calculate basic angle for 8-gon dReal fAngle = M_PI / nCYLINDER_CIRCLE_SEGMENTS; // calculate angle increment dReal fAngleIncrement = fAngle*REAL(2.0); // calculate plane normals // axis dependant code for(int i=0; i<nCYLINDER_CIRCLE_SEGMENTS; i++) { cData.avCylinderNormals[i][0] = -dCos(fAngle); cData.avCylinderNormals[i][1] = -dSin(fAngle); cData.avCylinderNormals[i][2] = REAL(0.0); fAngle += fAngleIncrement; } dSetZero(cData.vBestPoint,4); // reset best depth cData.fBestCenter = REAL(0.0); }
// 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; }