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); }
inline int _ProcessLocalContacts(sData& cData) { if (cData.nContacts == 0) { return 0; } #ifdef OPTIMIZE_CONTACTS if (cData.nContacts > 1 && !(cData.iFlags & CONTACTS_UNIMPORTANT)) { // Can be optimized... _OptimizeLocalContacts(cData); } #endif int iContact = 0; dContactGeom* Contact = 0; int nFinalContact = 0; for (iContact = 0; iContact < cData.nContacts; iContact ++) { if (1 == cData.gLocalContacts[iContact].nFlags) { Contact = SAFECONTACT(cData.iFlags, cData.gContact, nFinalContact, cData.iSkip); Contact->depth = cData.gLocalContacts[iContact].fDepth; dVector3Copy(cData.gLocalContacts[iContact].vNormal,Contact->normal); dVector3Copy(cData.gLocalContacts[iContact].vPos,Contact->pos); Contact->g1 = cData.gCylinder; Contact->g2 = cData.gTrimesh; Contact->side2 = cData.gLocalContacts[iContact].triIndex; dVector3Inv(Contact->normal); nFinalContact++; } } // debug //if (nFinalContact != cData.nContacts) //{ // printf("[Info] %d contacts generated,%d filtered.\n",cData.nContacts,cData.nContacts-nFinalContact); //} return nFinalContact; }
// check for separation between box edge and cylinder circle edge int _cldTestEdgeCircleAxis( sCylinderBoxData& cData, const dVector3 &vCenterPoint, const dVector3 &vVx0, const dVector3 &vVx1, int iAxis ) { // calculate direction of edge dVector3 vDirEdge; dVector3Subtract(vVx1,vVx0,vDirEdge); dNormalize3(vDirEdge); // starting point of edge dVector3 vEStart; dVector3Copy(vVx0,vEStart);; // calculate angle cosine between cylinder axis and edge dReal fdot2 = dVector3Dot (vDirEdge,cData.vCylinderAxis); // if edge is perpendicular to cylinder axis if(dFabs(fdot2) < 1e-5f) { // this can't be separating axis, because edge is parallel to circle plane return 1; } // find point of intersection between edge line and circle plane dVector3 vTemp1; dVector3Subtract(vCenterPoint,vEStart,vTemp1); dReal fdot1 = dVector3Dot(vTemp1,cData.vCylinderAxis); dVector3 vpnt; vpnt[0]= vEStart[0] + vDirEdge[0] * (fdot1/fdot2); vpnt[1]= vEStart[1] + vDirEdge[1] * (fdot1/fdot2); vpnt[2]= vEStart[2] + vDirEdge[2] * (fdot1/fdot2); // find tangent vector on circle with same center (vCenterPoint) that // touches point of intersection (vpnt) dVector3 vTangent; dVector3Subtract(vCenterPoint,vpnt,vTemp1); dVector3Cross(vTemp1,cData.vCylinderAxis,vTangent); // find vector orthogonal both to tangent and edge direction dVector3 vAxis; dVector3Cross(vTangent,vDirEdge,vAxis); // use that vector as separating axis return _cldTestAxis( cData, vAxis, iAxis ); }
// intersection test between edge and circle bool _cldTestCircleToEdgeAxis(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, const dVector3 &vCenterPoint, const dVector3 &vCylinderAxis1, const dVector3 &vVx0, const dVector3 &vVx1, int iAxis) { // calculate direction of edge dVector3 vkl; dVector3Subtract( vVx1 , vVx0 , vkl); dNormalize3(vkl); // starting point of edge dVector3 vol; dVector3Copy(vVx0,vol); // calculate angle cosine between cylinder axis and edge dReal fdot2 = dVector3Dot(vkl , vCylinderAxis1); // if edge is perpendicular to cylinder axis if(dFabs(fdot2)<REAL(1e-5)) { // this can't be separating axis, because edge is parallel to circle plane return true; } // find point of intersection between edge line and circle plane dVector3 vTemp; dVector3Subtract(vCenterPoint,vol,vTemp); dReal fdot1 = dVector3Dot(vTemp,vCylinderAxis1); dVector3 vpnt;// = vol + vkl * (fdot1/fdot2); vpnt[0] = vol[0] + vkl[0] * fdot1/fdot2; vpnt[1] = vol[1] + vkl[1] * fdot1/fdot2; vpnt[2] = vol[2] + vkl[2] * fdot1/fdot2; // find tangent vector on circle with same center (vCenterPoint) that touches point of intersection (vpnt) dVector3 vTangent; dVector3Subtract(vCenterPoint,vpnt,vTemp); dVector3Cross(vTemp,vCylinderAxis1,vTangent); // find vector orthogonal both to tangent and edge direction dVector3 vAxis; dVector3Cross(vTangent,vkl,vAxis); // use that vector as separating axis return _cldTestAxis( cData ,v0, v1, v2, vAxis, iAxis ); }
bool _cldTestSeparatingAxes(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { // calculate edge vectors dVector3Subtract(v1 ,v0 , cData.vE0); // cData.vE1 has been calculated before -> so save some cycles here dVector3Subtract(v0 ,v2 , cData.vE2); // calculate caps centers in absolute space dVector3 vCp0; vCp0[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); vCp0[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); vCp0[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); dVector3 vCp1; vCp1[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize* REAL(0.5)); vCp1[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize* REAL(0.5)); vCp1[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize* REAL(0.5)); // reset best axis cData.iBestAxis = 0; dVector3 vAxis; // axis cData.vNormal //vAxis = -cData.vNormal; vAxis[0] = -cData.vNormal[0]; vAxis[1] = -cData.vNormal[1]; vAxis[2] = -cData.vNormal[2]; if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 1, true)) { return false; } // axis CxE0 // vAxis = ( cData.vCylinderAxis cross cData.vE0 ); dVector3Cross(cData.vCylinderAxis, cData.vE0,vAxis); if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 2)) { return false; } // axis CxE1 // vAxis = ( cData.vCylinderAxis cross cData.vE1 ); dVector3Cross(cData.vCylinderAxis, cData.vE1,vAxis); if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 3)) { return false; } // axis CxE2 // vAxis = ( cData.vCylinderAxis cross cData.vE2 ); dVector3Cross(cData.vCylinderAxis, cData.vE2,vAxis); if (!_cldTestAxis( cData ,v0, v1, v2, vAxis, 4)) { return false; } // first vertex on triangle // axis ((V0-Cp0) x C) x C //vAxis = ( ( v0-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; _CalculateAxis(v0 , vCp0 , cData.vCylinderAxis , vAxis); if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 11)) { return false; } // second vertex on triangle // axis ((V1-Cp0) x C) x C // vAxis = ( ( v1-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; _CalculateAxis(v1 , vCp0 , cData.vCylinderAxis , vAxis); if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 12)) { return false; } // third vertex on triangle // axis ((V2-Cp0) x C) x C //vAxis = ( ( v2-vCp0 ) cross cData.vCylinderAxis ) cross cData.vCylinderAxis; _CalculateAxis(v2 , vCp0 , cData.vCylinderAxis , vAxis); if (!_cldTestAxis(cData, v0, v1, v2, vAxis, 13)) { return false; } // test cylinder axis // vAxis = cData.vCylinderAxis; dVector3Copy(cData.vCylinderAxis , vAxis); if (!_cldTestAxis(cData , v0, v1, v2, vAxis, 14)) { return false; } // Test top and bottom circle ring of cylinder for separation dVector3 vccATop; vccATop[0] = cData.vCylinderPos[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); vccATop[1] = cData.vCylinderPos[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); vccATop[2] = cData.vCylinderPos[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); dVector3 vccABottom; vccABottom[0] = cData.vCylinderPos[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize * REAL(0.5)); vccABottom[1] = cData.vCylinderPos[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize * REAL(0.5)); vccABottom[2] = cData.vCylinderPos[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize * REAL(0.5)); if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v1, 15)) { return false; } if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v1, v2, 16)) { return false; } if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccATop, cData.vCylinderAxis, v0, v2, 17)) { return false; } if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v1, 18)) { return false; } if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v1, v2, 19)) { return false; } if (!_cldTestCircleToEdgeAxis(cData, v0, v1, v2, vccABottom, cData.vCylinderAxis, v0, v2, 20)) { return false; } return true; }
bool _cldTestAxis(sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, dVector3& vAxis, int iAxis, bool bNoFlip = false) { // calculate length of separating axis vector dReal fL = dVector3Length(vAxis); // if not long enough if ( fL < REAL(1e-5) ) { // do nothing return true; } // otherwise normalize it vAxis[0] /= fL; vAxis[1] /= fL; vAxis[2] /= fL; dReal fdot1 = dVector3Dot(cData.vCylinderAxis,vAxis); // project capsule on vAxis dReal frc; if (dFabs(fdot1) > REAL(1.0) ) { // fdot1 = REAL(1.0); frc = dFabs(cData.fCylinderSize* REAL(0.5)); } else { frc = dFabs((cData.fCylinderSize* REAL(0.5)) * fdot1) + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); } dVector3 vV0; dVector3Subtract(v0,cData.vCylinderPos,vV0); dVector3 vV1; dVector3Subtract(v1,cData.vCylinderPos,vV1); dVector3 vV2; dVector3Subtract(v2,cData.vCylinderPos,vV2); // project triangle on vAxis dReal afv[3]; afv[0] = dVector3Dot( vV0 , vAxis ); afv[1] = dVector3Dot( vV1 , vAxis ); afv[2] = dVector3Dot( vV2 , vAxis ); dReal fMin = MAX_REAL; dReal fMax = -MAX_REAL; // for each vertex for(int i = 0; i < 3; i++) { // find minimum if (afv[i]<fMin) { fMin = afv[i]; } // find maximum if (afv[i]>fMax) { fMax = afv[i]; } } // find capsule's center of interval on axis dReal fCenter = (fMin+fMax)* REAL(0.5); // calculate triangles halfinterval dReal fTriangleRadius = (fMax-fMin)*REAL(0.5); // if they do not overlap, if( dFabs(fCenter) > (frc+fTriangleRadius) ) { // exit, we have no intersection return false; } // calculate depth dReal fDepth = -(dFabs(fCenter) - (frc + fTriangleRadius ) ); // if greater then best found so far if ( fDepth < cData.fBestDepth ) { // remember depth cData.fBestDepth = fDepth; cData.fBestCenter = fCenter; cData.fBestrt = frc; dVector3Copy(vAxis,cData.vContactNormal); cData.iBestAxis = iAxis; // flip normal if interval is wrong faced if ( fCenter< REAL(0.0) && !bNoFlip) { dVector3Inv(cData.vContactNormal); cData.fBestCenter = -fCenter; } } return true; }
int dCollideSTL(dxGeom* g1, dxGeom* SphereGeom, int Flags, dContactGeom* Contacts, int Stride){ dIASSERT (Stride >= (int)sizeof(dContactGeom)); dIASSERT (g1->type == dTriMeshClass); dIASSERT (SphereGeom->type == dSphereClass); dIASSERT ((Flags & NUMC_MASK) >= 1); dxTriMesh* TriMesh = (dxTriMesh*)g1; // Init const dVector3& TLPosition = *(const dVector3*)dGeomGetPosition(TriMesh); const dMatrix3& TLRotation = *(const dMatrix3*)dGeomGetRotation(TriMesh); TrimeshCollidersCache *pccColliderCache = GetTrimeshCollidersCache(); SphereCollider& Collider = pccColliderCache->_SphereCollider; const dVector3& Position = *(const dVector3*)dGeomGetPosition(SphereGeom); dReal Radius = dGeomSphereGetRadius(SphereGeom); // Sphere Sphere Sphere; Sphere.mCenter.x = Position[0]; Sphere.mCenter.y = Position[1]; Sphere.mCenter.z = Position[2]; Sphere.mRadius = Radius; Matrix4x4 amatrix; // TC results if (TriMesh->doSphereTC) { dxTriMesh::SphereTC* sphereTC = 0; for (int i = 0; i < TriMesh->SphereTCCache.size(); i++){ if (TriMesh->SphereTCCache[i].Geom == SphereGeom){ sphereTC = &TriMesh->SphereTCCache[i]; break; } } if (!sphereTC){ TriMesh->SphereTCCache.push(dxTriMesh::SphereTC()); sphereTC = &TriMesh->SphereTCCache[TriMesh->SphereTCCache.size() - 1]; sphereTC->Geom = SphereGeom; } // Intersect Collider.SetTemporalCoherence(true); Collider.Collide(*sphereTC, Sphere, TriMesh->Data->BVTree, null, &MakeMatrix(TLPosition, TLRotation, amatrix)); } else { Collider.SetTemporalCoherence(false); Collider.Collide(pccColliderCache->defaultSphereCache, Sphere, TriMesh->Data->BVTree, null, &MakeMatrix(TLPosition, TLRotation, amatrix)); } if (! Collider.GetContactStatus()) { // no collision occurred return 0; } // get results int TriCount = Collider.GetNbTouchedPrimitives(); const int* Triangles = (const int*)Collider.GetTouchedPrimitives(); if (TriCount != 0){ if (TriMesh->ArrayCallback != null){ TriMesh->ArrayCallback(TriMesh, SphereGeom, Triangles, TriCount); } int OutTriCount = 0; for (int i = 0; i < TriCount; i++){ if (OutTriCount == (Flags & NUMC_MASK)){ break; } const int TriIndex = Triangles[i]; dVector3 dv[3]; if (!Callback(TriMesh, SphereGeom, TriIndex)) continue; FetchTriangle(TriMesh, TriIndex, TLPosition, TLRotation, dv); dVector3& v0 = dv[0]; dVector3& v1 = dv[1]; dVector3& v2 = dv[2]; dVector3 vu; vu[0] = v1[0] - v0[0]; vu[1] = v1[1] - v0[1]; vu[2] = v1[2] - v0[2]; vu[3] = REAL(0.0); dVector3 vv; vv[0] = v2[0] - v0[0]; vv[1] = v2[1] - v0[1]; vv[2] = v2[2] - v0[2]; vv[3] = REAL(0.0); // Get plane coefficients dVector4 Plane; dCROSS(Plane, =, vu, vv); // Even though all triangles might be initially valid, // a triangle may degenerate into a segment after applying // space transformation. if (!dSafeNormalize3(Plane)) { continue; } /* If the center of the sphere is within the positive halfspace of the * triangle's plane, allow a contact to be generated. * If the center of the sphere made it into the positive halfspace of a * back-facing triangle, then the physics update and/or velocity needs * to be adjusted (penetration has occured anyway). */ dReal side = dDOT(Plane,Position) - dDOT(Plane, v0); if(side < REAL(0.0)) { continue; } dReal Depth; dReal u, v; if (!GetContactData(Position, Radius, v0, vu, vv, Depth, u, v)){ continue; // Sphere doesn't hit triangle } if (Depth < REAL(0.0)){ continue; // Negative depth does not produce a contact } dVector3 ContactPos; dReal w = REAL(1.0) - u - v; ContactPos[0] = (v0[0] * w) + (v1[0] * u) + (v2[0] * v); ContactPos[1] = (v0[1] * w) + (v1[1] * u) + (v2[1] * v); ContactPos[2] = (v0[2] * w) + (v1[2] * u) + (v2[2] * v); // Depth returned from GetContactData is depth along // contact point - sphere center direction // we'll project it to contact normal dVector3 dir; dir[0] = Position[0]-ContactPos[0]; dir[1] = Position[1]-ContactPos[1]; dir[2] = Position[2]-ContactPos[2]; dReal dirProj = dDOT(dir, Plane) / dSqrt(dDOT(dir, dir)); // Since Depth already had a requirement to be non-negative, // negative direction projections should not be allowed as well, // as otherwise the multiplication will result in negative contact depth. if (dirProj < REAL(0.0)){ continue; // Zero contact depth could be ignored } dContactGeom* Contact = SAFECONTACT(Flags, Contacts, OutTriCount, Stride); Contact->pos[0] = ContactPos[0]; Contact->pos[1] = ContactPos[1]; Contact->pos[2] = ContactPos[2]; Contact->pos[3] = REAL(0.0); // Using normal as plane (reversed) Contact->normal[0] = -Plane[0]; Contact->normal[1] = -Plane[1]; Contact->normal[2] = -Plane[2]; Contact->normal[3] = REAL(0.0); Contact->depth = Depth * dirProj; //Contact->depth = Radius - side; // (mg) penetration depth is distance along normal not shortest distance #if !defined MERGECONTACTS // Merge all contacts into 1 Contact->g1 = TriMesh; Contact->g2 = SphereGeom; Contact->side2 = -1; #endif // Otherwise assigned later Contact->side1 = TriIndex; OutTriCount++; } #if defined MERGECONTACTS // Merge all contacts into 1 if (OutTriCount > 0){ dContactGeom* Contact = SAFECONTACT(Flags, Contacts, 0, Stride); Contact->g1 = TriMesh; Contact->g2 = SphereGeom; Contact->side2 = -1; if (OutTriCount > 1 && !(Flags & CONTACTS_UNIMPORTANT)){ dVector3 pos; pos[0] = Contact->pos[0]; pos[1] = Contact->pos[1]; pos[2] = Contact->pos[2]; dVector3 normal; normal[0] = Contact->normal[0] * Contact->depth; normal[1] = Contact->normal[1] * Contact->depth; normal[2] = Contact->normal[2] * Contact->depth; int TriIndex = Contact->side1; for (int i = 1; i < OutTriCount; i++){ dContactGeom* TempContact = SAFECONTACT(Flags, Contacts, i, Stride); pos[0] += TempContact->pos[0]; pos[1] += TempContact->pos[1]; pos[2] += TempContact->pos[2]; normal[0] += TempContact->normal[0] * TempContact->depth; normal[1] += TempContact->normal[1] * TempContact->depth; normal[2] += TempContact->normal[2] * TempContact->depth; TriIndex = (TriMesh->TriMergeCallback) ? TriMesh->TriMergeCallback(TriMesh, TriIndex, TempContact->side1) : -1; } Contact->side1 = TriIndex; Contact->pos[0] = pos[0] / OutTriCount; Contact->pos[1] = pos[1] / OutTriCount; Contact->pos[2] = pos[2] / OutTriCount; // Remember to divide in square space. Contact->depth = dSqrt(dDOT(normal, normal) / OutTriCount); if (Contact->depth > dEpsilon) { // otherwise the normal is too small dVector3Copy(Contact->normal, normal); dNormalize3(Contact->normal); } // otherwise original Contact's normal would be used and it should be already normalized } return 1; } else return 0; #elif defined MERGECONTACTNORMALS // Merge all normals, and distribute between all contacts if (OutTriCount != 0){ if (OutTriCount != 1 && !(Flags & CONTACTS_UNIMPORTANT)){ dVector3 Normal; dContactGeom* FirstContact = SAFECONTACT(Flags, Contacts, 0, Stride); Normal[0] = FirstContact->normal[0] * FirstContact->depth; Normal[1] = FirstContact->normal[1] * FirstContact->depth; Normal[2] = FirstContact->normal[2] * FirstContact->depth; Normal[3] = FirstContact->normal[3] * FirstContact->depth; for (int i = 1; i < OutTriCount; i++){ dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); Normal[0] += Contact->normal[0] * Contact->depth; Normal[1] += Contact->normal[1] * Contact->depth; Normal[2] += Contact->normal[2] * Contact->depth; Normal[3] += Contact->normal[3] * Contact->depth; } dNormalize3(Normal); for (int i = 0; i < OutTriCount; i++){ dContactGeom* Contact = SAFECONTACT(Flags, Contacts, i, Stride); Contact->normal[0] = Normal[0]; Contact->normal[1] = Normal[1]; Contact->normal[2] = Normal[2]; Contact->normal[3] = Normal[3]; } } return OutTriCount; } else return 0; #else // none of MERGECONTACTS and MERGECONTACTNORMALS // Just return return OutTriCount; #endif // MERGECONTACTS } else return 0;
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; }
void TestOneTriangleVsCylinder( sData& cData, const dVector3 &v0, const dVector3 &v1, const dVector3 &v2, const bool bDoubleSided) { // calculate triangle normal dVector3Subtract( v2 , v1 ,cData.vE1); dVector3 vTemp; dVector3Subtract( v0 , v1 ,vTemp); dVector3Cross(cData.vE1 , vTemp , cData.vNormal ); dNormalize3( cData.vNormal); // create plane from triangle //Plane4f plTrianglePlane = Plane4f( vPolyNormal, v0 ); dReal plDistance = -dVector3Dot(v0, cData.vNormal); dVector4 plTrianglePlane; dConstructPlane( cData.vNormal,plDistance,plTrianglePlane); // calculate sphere distance to plane dReal fDistanceCylinderCenterToPlane = dPointPlaneDistance(cData.vCylinderPos , plTrianglePlane); // Sphere must be over positive side of triangle if(fDistanceCylinderCenterToPlane < 0 && !bDoubleSided) { // if not don't generate contacts return; } dVector3 vPnt0; dVector3 vPnt1; dVector3 vPnt2; if (fDistanceCylinderCenterToPlane < REAL(0.0) ) { // flip it dVector3Copy(v0 , vPnt0); dVector3Copy(v1 , vPnt2); dVector3Copy(v2 , vPnt1); } else { dVector3Copy(v0 , vPnt0); dVector3Copy(v1 , vPnt1); dVector3Copy(v2 , vPnt2); } cData.fBestDepth = MAX_REAL; // do intersection test and find best separating axis if(!_cldTestSeparatingAxes(cData , vPnt0, vPnt1, vPnt2) ) { // if not found do nothing return; } // if best separation axis is not found if ( cData.iBestAxis == 0 ) { // this should not happen (we should already exit in that case) dIASSERT(false); // do nothing return; } dReal fdot = dVector3Dot( cData.vContactNormal , cData.vCylinderAxis ); // choose which clipping method are we going to apply if (dFabs(fdot) < REAL(0.9) ) { if (!_cldClipCylinderEdgeToTriangle(cData ,vPnt0, vPnt1, vPnt2)) { return; } } else { _cldClipCylinderToTriangle(cData ,vPnt0, vPnt1, vPnt2); } }
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; }
// Test separating axis for collision int _cldTestSeparatingAxes(sCylinderBoxData& cData) { // reset best axis cData.fBestDepth = MAX_FLOAT; cData.iBestAxis = 0; cData.fBestrb = 0; cData.fBestrc = 0; cData.nContacts = 0; dVector3 vAxis = {REAL(0.0),REAL(0.0),REAL(0.0),REAL(0.0)}; // Epsilon value for checking axis vector length const dReal fEpsilon = 1e-6f; // axis A0 dMat3GetCol(cData.mBoxRot, 0 , vAxis); if (!_cldTestAxis( cData, vAxis, 1 )) { return 0; } // axis A1 dMat3GetCol(cData.mBoxRot, 1 , vAxis); if (!_cldTestAxis( cData, vAxis, 2 )) { return 0; } // axis A2 dMat3GetCol(cData.mBoxRot, 2 , vAxis); if (!_cldTestAxis( cData, vAxis, 3 )) { return 0; } // axis C - Cylinder Axis //vAxis = vCylinderAxis; dVector3Copy(cData.vCylinderAxis , vAxis); if (!_cldTestAxis( cData, vAxis, 4 )) { return 0; } // axis CxA0 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 0 )); dVector3CrossMat3Col(cData.mBoxRot, 0 ,cData.vCylinderAxis, vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 5 )) { return 0; } } // axis CxA1 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 1 )); dVector3CrossMat3Col(cData.mBoxRot, 1 ,cData.vCylinderAxis, vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 6 )) { return 0; } } // axis CxA2 //vAxis = ( vCylinderAxis cross mthGetColM33f( mBoxRot, 2 )); dVector3CrossMat3Col(cData.mBoxRot, 2 ,cData.vCylinderAxis, vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 7 )) { return 0; } } int i = 0; dVector3 vTemp1; dVector3 vTemp2; // here we check box's vertices axis for(i=0; i< 8; i++) { //vAxis = ( vCylinderAxis cross (cData.avBoxVertices[i] - vCylinderPos)); dVector3Subtract(cData.avBoxVertices[i],cData.vCylinderPos,vTemp1); dVector3Cross(cData.vCylinderAxis,vTemp1,vTemp2); //vAxis = ( vCylinderAxis cross vAxis ); dVector3Cross(cData.vCylinderAxis,vTemp2,vAxis); if(dVector3Length2( vAxis ) > fEpsilon ) { if (!_cldTestAxis( cData, vAxis, 8 + i )) { return 0; } } } // ************************************ // this is defined for first 12 axes // normal of plane that contains top circle of cylinder // center of top circle of cylinder dVector3 vcc; vcc[0] = (cData.vCylinderPos)[0] + cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vcc[1] = (cData.vCylinderPos)[1] + cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vcc[2] = (cData.vCylinderPos)[2] + cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); // ************************************ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 16)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 17)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 18)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 19)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 20)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 21)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 22)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 23)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 24)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 25)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 26)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 27)) { return 0; } // ************************************ // this is defined for second 12 axes // normal of plane that contains bottom circle of cylinder // center of bottom circle of cylinder // vcc = vCylinderPos - vCylinderAxis*(fCylinderSize*REAL(0.5)); vcc[0] = (cData.vCylinderPos)[0] - cData.vCylinderAxis[0]*(cData.fCylinderSize*REAL(0.5)); vcc[1] = (cData.vCylinderPos)[1] - cData.vCylinderAxis[1]*(cData.fCylinderSize*REAL(0.5)); vcc[2] = (cData.vCylinderPos)[2] - cData.vCylinderAxis[2]*(cData.fCylinderSize*REAL(0.5)); // ************************************ if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[0], 28)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[1], cData.avBoxVertices[3], 29)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[3], 30)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[0], 31)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[1], 32)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[7], 33)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[0], cData.avBoxVertices[7], 34)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[3], 35)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[5], cData.avBoxVertices[6], 36)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[2], cData.avBoxVertices[6], 37)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[4], cData.avBoxVertices[5], 38)) { return 0; } if (!_cldTestEdgeCircleAxis( cData, vcc, cData.avBoxVertices[6], cData.avBoxVertices[7], 39)) { return 0; } return 1; }
// test for given separating axis int _cldTestAxis(sCylinderBoxData& cData, dVector3& vInputNormal, int iAxis ) { // check length of input normal dReal fL = dVector3Length(vInputNormal); // if not long enough if ( fL < 1e-5f ) { // do nothing return 1; } // otherwise make it unit for sure dNormalize3(vInputNormal); // project box and Cylinder on mAxis dReal fdot1 = dVector3Dot(cData.vCylinderAxis, vInputNormal); dReal frc; if (fdot1 > REAL(1.0)) { fdot1 = REAL(1.0); frc = dFabs(cData.fCylinderSize*REAL(0.5)); } // project box and capsule on iAxis frc = dFabs( fdot1 * (cData.fCylinderSize*REAL(0.5))) + cData.fCylinderRadius * dSqrt(REAL(1.0)-(fdot1*fdot1)); dVector3 vTemp1; dReal frb = REAL(0.0); dMat3GetCol(cData.mBoxRot,0,vTemp1); frb = dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[0]; dMat3GetCol(cData.mBoxRot,1,vTemp1); frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[1]; dMat3GetCol(cData.mBoxRot,2,vTemp1); frb += dFabs(dVector3Dot(vTemp1,vInputNormal))*cData.vBoxHalfSize[2]; // project their distance on separating axis dReal fd = dVector3Dot(cData.vDiff,vInputNormal); // if they do not overlap exit, we have no intersection if ( dFabs(fd) > frc+frb ) { return 0; } // get depth dReal fDepth = - dFabs(fd) + (frc+frb); // get maximum depth if ( fDepth < cData.fBestDepth ) { cData.fBestDepth = fDepth; dVector3Copy(vInputNormal,cData.vNormal); cData.iBestAxis = iAxis; cData.fBestrb = frb; cData.fBestrc = frc; // flip normal if interval is wrong faced if (fd > 0) { dVector3Inv(cData.vNormal); } } return 1; }
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; 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; 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; 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; 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; 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; GeomCount++; if( GeomCount >= (flags & NUMC_MASK)) return GeomCount; // enough contactgeoms contact = (dContactGeom *)((char *)contact + skip); } } return GeomCount; }
// 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; }
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; }
void _cldClipCylinderToTriangle(sData& cData,const dVector3 &v0, const dVector3 &v1, const dVector3 &v2) { int i = 0; dVector3 avPoints[3]; dVector3 avTempArray1[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; dVector3 avTempArray2[nMAX_CYLINDER_TRIANGLE_CLIP_POINTS]; dSetZero(&avTempArray1[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); dSetZero(&avTempArray2[0][0],nMAX_CYLINDER_TRIANGLE_CLIP_POINTS * 4); // setup array of triangle vertices dVector3Copy(v0,avPoints[0]); dVector3Copy(v1,avPoints[1]); dVector3Copy(v2,avPoints[2]); dVector3 vCylinderCirclePos, vCylinderCircleNormal_Rel; dSetZero(vCylinderCircleNormal_Rel,4); // check which circle from cylinder we take for clipping if ( dVector3Dot(cData.vCylinderAxis , cData.vContactNormal) > 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[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[nCYLINDER_AXIS] = REAL(1.0); } dVector3 vTemp; dQuatInv(cData.qCylinderRot , cData.qInvCylinderRot); // transform triangle points to space of cylinder circle for(i=0; i<3; i++) { dVector3Subtract(avPoints[i] , vCylinderCirclePos , vTemp); dQuatTransform(cData.qInvCylinderRot,vTemp,avPoints[i]); } int iTmpCounter1 = 0; int iTmpCounter2 = 0; dVector4 plPlane; // plane of cylinder that contains circle for intersection //plPlane = Plane4f( vCylinderCircleNormal_Rel, 0.0f ); dConstructPlane(vCylinderCircleNormal_Rel,REAL(0.0),plPlane); dClipPolyToPlane(avPoints, 3, avTempArray1, iTmpCounter1, plPlane); // Body of base circle of Cylinder int nCircleSegment = 0; for (nCircleSegment = 0; nCircleSegment < nCYLINDER_CIRCLE_SEGMENTS; 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 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); dIASSERT( iTmpCounter2 >= 0 && iTmpCounter2 <= nMAX_CYLINDER_TRIANGLE_CLIP_POINTS ); } // back transform clipped points to absolute space dReal ftmpdot; dReal fTempDepth; dVector3 vPoint; if (nCircleSegment %2) { for( i=0; i<iTmpCounter2; i++) { dQuatTransform(cData.qCylinderRot,avTempArray2[i], vPoint); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal)); fTempDepth = cData.fBestrt - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); cData.gLocalContacts[cData.nContacts].nFlags = 1; cData.nContacts++; if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) return;; } } } else { for( i=0; i<iTmpCounter1; i++) { dQuatTransform(cData.qCylinderRot,avTempArray1[i], vPoint); vPoint[0] += vCylinderCirclePos[0]; vPoint[1] += vCylinderCirclePos[1]; vPoint[2] += vCylinderCirclePos[2]; dVector3Subtract(vPoint,cData.vCylinderPos,vTemp); ftmpdot = dFabs(dVector3Dot(vTemp, cData.vContactNormal)); fTempDepth = cData.fBestrt - ftmpdot; // Depth must be positive if (fTempDepth > REAL(0.0)) { cData.gLocalContacts[cData.nContacts].fDepth = fTempDepth; dVector3Copy(cData.vContactNormal,cData.gLocalContacts[cData.nContacts].vNormal); dVector3Copy(vPoint,cData.gLocalContacts[cData.nContacts].vPos); cData.gLocalContacts[cData.nContacts].nFlags = 1; cData.nContacts++; if(cData.nContacts >= (cData.iFlags & NUMC_MASK)) return;; } } } }
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; }
// 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; }