// test for given separating axis int sCylinderBoxData::_cldTestAxis( dVector3& vInputNormal, int iAxis ) { // check length of input normal dReal fL = dVector3Length(vInputNormal); // if not long enough if ( fL < REAL(1e-5) ) { // do nothing return 1; } // otherwise make it unit for sure dNormalize3(vInputNormal); // project box and Cylinder on mAxis dReal fdot1 = dVector3Dot(m_vCylinderAxis, vInputNormal); dReal frc; if (fdot1 > REAL(1.0)) { // assume fdot1 = 1 frc = m_fCylinderSize*REAL(0.5); } else if (fdot1 < REAL(-1.0)) { // assume fdot1 = -1 frc = m_fCylinderSize*REAL(0.5); } else { // project box and capsule on iAxis frc = dFabs( fdot1 * (m_fCylinderSize*REAL(0.5))) + m_fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); } dVector3 vTemp1; dMat3GetCol(m_mBoxRot,0,vTemp1); dReal frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[0]; dMat3GetCol(m_mBoxRot,1,vTemp1); frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[1]; dMat3GetCol(m_mBoxRot,2,vTemp1); frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*m_vBoxHalfSize[2]; // project their distance on separating axis dReal fd = dVector3Dot(m_vDiff,vInputNormal); // get depth dReal fDepth = frc + frb; // Calculate partial depth // if they do not overlap exit, we have no intersection if ( dFabs(fd) > fDepth ) { return 0; } // Finalyze the depth calculation fDepth -= dFabs(fd); // get maximum depth if ( fDepth < m_fBestDepth ) { m_fBestDepth = fDepth; dVector3Copy(vInputNormal,m_vNormal); m_iBestAxis = iAxis; m_fBestrb = frb; m_fBestrc = frc; // flip normal if interval is wrong faced if (fd > 0) { dVector3Inv(m_vNormal); } } return 1; }
void sCylinderBoxData::_cldClipBoxToCylinder() { dIASSERT(m_nContacts != (m_iFlags & NUMC_MASK)); dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; // check which circle from cylinder we take for clipping if ( dVector3Dot(m_vCylinderAxis, m_vNormal) > REAL(0.0) ) { // get top circle vCylinderCirclePos[0] = m_vCylinderPos[0] + m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = m_vCylinderPos[1] + m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = m_vCylinderPos[2] + m_vCylinderAxis[2]*(m_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] = m_vCylinderPos[0] - m_vCylinderAxis[0]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[1] = m_vCylinderPos[1] - m_vCylinderAxis[1]*(m_fCylinderSize*REAL(0.5)); vCylinderCirclePos[2] = m_vCylinderPos[2] - m_vCylinderAxis[2]*(m_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(m_mBoxRot,mBoxInv); dMultiplyMat3Vec3(mBoxInv,m_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(m_mBoxRot,iB0,vTemp); vCenter[0] = m_vBoxPos[0] - m_vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = m_vBoxPos[1] - m_vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = m_vBoxPos[2] - m_vBoxHalfSize[iB0]*vTemp[2]; } else { dMat3GetCol(m_mBoxRot,iB0,vTemp); vCenter[0] = m_vBoxPos[0] + m_vBoxHalfSize[iB0]*vTemp[0]; vCenter[1] = m_vBoxPos[1] + m_vBoxHalfSize[iB0]*vTemp[1]; vCenter[2] = m_vBoxPos[2] + m_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(m_mBoxRot,iB1,vAxis1); dMat3GetCol(m_mBoxRot,iB2,vAxis2); avPoints[0][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[0][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[0][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2]; avPoints[1][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] - m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[1][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] - m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[1][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] - m_vBoxHalfSize[iB2] * vAxis2[2]; avPoints[2][0] = vCenter[0] - m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[2][1] = vCenter[1] - m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[2][2] = vCenter[2] - m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2]; avPoints[3][0] = vCenter[0] + m_vBoxHalfSize[iB1] * vAxis1[0] + m_vBoxHalfSize[iB2] * vAxis2[0]; avPoints[3][1] = vCenter[1] + m_vBoxHalfSize[iB1] * vAxis1[1] + m_vBoxHalfSize[iB2] * vAxis2[1]; avPoints[3][2] = vCenter[2] + m_vBoxHalfSize[iB1] * vAxis1[2] + m_vBoxHalfSize[iB2] * vAxis2[2]; // transform box points to space of cylinder circle dMatrix3 mCylinderInv; dMatrix3Inv(m_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(m_avCylinderNormals[nCircleSegment],m_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,m_mCylinderRot,avTempArray2[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,m_vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, m_vNormal); fTempDepth = m_fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); Contact0->depth = fTempDepth; dVector3Copy(m_vNormal,Contact0->normal); dVector3Copy(vPoint,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)) { break; } } } } else { for( i=0; i<iTmpCounter1; i++) { dMultiply0_331(vPoint,m_mCylinderRot,avTempArray1[i]); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,m_vCylinderPos,vTemp); ftmpdot = dVector3Dot(vTemp, m_vNormal); fTempDepth = m_fBestrc - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { // generate contacts dContactGeom* Contact0 = SAFECONTACT(m_iFlags, m_gContact, m_nContacts, m_iSkip); Contact0->depth = fTempDepth; dVector3Copy(m_vNormal,Contact0->normal); dVector3Copy(vPoint,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)) { break; } } } } }
int dCollideCylinderPlane(dxGeom *Cylinder, dxGeom *Plane, int flags, dContactGeom *contact, int skip) { dIASSERT (skip >= (int)sizeof(dContactGeom)); dIASSERT (Cylinder->type == dCylinderClass); dIASSERT (Plane->type == dPlaneClass); dIASSERT ((flags & NUMC_MASK) >= 1); int GeomCount = 0; // count of used contactgeoms #ifdef dSINGLE const dReal toleranz = REAL(0.0001); #endif #ifdef dDOUBLE const dReal toleranz = REAL(0.0000001); #endif // Get the properties of the cylinder (length+radius) dReal radius, length; dGeomCylinderGetParams(Cylinder, &radius, &length); dVector3 &cylpos = Cylinder->final_posr->pos; // and the plane dVector4 planevec; dGeomPlaneGetParams(Plane, planevec); dVector3 PlaneNormal = {planevec[0],planevec[1],planevec[2]}; //dVector3 PlanePos = {planevec[0] * planevec[3],planevec[1] * planevec[3],planevec[2] * planevec[3]}; dVector3 G1Pos1, G1Pos2, vDir1; vDir1[0] = Cylinder->final_posr->R[2]; vDir1[1] = Cylinder->final_posr->R[6]; vDir1[2] = Cylinder->final_posr->R[10]; dReal s; s = length * REAL(0.5); G1Pos2[0] = vDir1[0] * s + cylpos[0]; G1Pos2[1] = vDir1[1] * s + cylpos[1]; G1Pos2[2] = vDir1[2] * s + cylpos[2]; G1Pos1[0] = vDir1[0] * -s + cylpos[0]; G1Pos1[1] = vDir1[1] * -s + cylpos[1]; G1Pos1[2] = vDir1[2] * -s + cylpos[2]; dVector3 C; // parallel-check s = vDir1[0] * PlaneNormal[0] + vDir1[1] * PlaneNormal[1] + vDir1[2] * PlaneNormal[2]; if(s < 0) s += REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel else s -= REAL(1.0); // is ca. 0, if vDir1 and PlaneNormal are parallel if(s < toleranz && s > (-toleranz)) { // discs are parallel to the plane // 1.compute if, and where contacts are dVector3 P; s = planevec[3] - dVector3Dot(planevec, G1Pos1); dReal t; t = planevec[3] - dVector3Dot(planevec, G1Pos2); if(s >= t) // s == t does never happen, { if(s >= 0) { // 1. Disc dVector3Copy(G1Pos1, P); } else return GeomCount; // no contacts } else { if(t >= 0) { // 2. Disc dVector3Copy(G1Pos2, P); } else return GeomCount; // no contacts } // 2. generate a coordinate-system on the disc dVector3 V1, V2; if(vDir1[0] < toleranz && vDir1[0] > (-toleranz)) { // not x-axis V1[0] = vDir1[0] + REAL(1.0); // random value V1[1] = vDir1[1]; V1[2] = vDir1[2]; } else { // maybe x-axis V1[0] = vDir1[0]; V1[1] = vDir1[1] + REAL(1.0); // random value V1[2] = vDir1[2]; } // V1 is now another direction than vDir1 // Cross-product dVector3Cross(V1, vDir1, V2); // make unit V2 t = dVector3Length(V2); t = radius / t; dVector3Scale(V2, t); // cross again dVector3Cross(V2, vDir1, V1); // |V2| is 'radius' and vDir1 unit, so |V1| is 'radius' // V1 = first axis // V2 = second axis // 3. generate contactpoints // Potential contact 1 dVector3Add(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 2 dVector3Subtract(P, V1, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 3 dVector3Add(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // Potential contact 4 dVector3Subtract(P, V2, contact->pos); contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth > 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } else { dReal t = dVector3Dot(PlaneNormal, vDir1); C[0] = vDir1[0] * t - PlaneNormal[0]; C[1] = vDir1[1] * t - PlaneNormal[1]; C[2] = vDir1[2] * t - PlaneNormal[2]; s = dVector3Length(C); // move C onto the circle s = radius / s; dVector3Scale(C, s); // deepest point of disc 1 dVector3Add(C, G1Pos1, contact->pos); // depth of the deepest point contact->depth = planevec[3] - dVector3Dot(planevec, contact->pos); if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } // C is still computed // deepest point of disc 2 dVector3Add(C, G1Pos2, contact->pos); // depth of the deepest point contact->depth = planevec[3] - planevec[0] * contact->pos[0] - planevec[1] * contact->pos[1] - planevec[2] * contact->pos[2]; if(contact->depth >= 0) { dVector3Copy(PlaneNormal, contact->normal); contact->g1 = Cylinder; contact->g2 = Plane; contact->side1 = -1; contact->side2 = -1; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } return GeomCount; }
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; }