Changeset View
Changeset View
Standalone View
Standalone View
extern/bullet2/src/LinearMath/btGeometryUtil.cpp
| /* | /* | ||||
| Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ | Copyright (c) 2003-2006 Gino van den Bergen / Erwin Coumans http://continuousphysics.com/Bullet/ | ||||
| This software is provided 'as-is', without any express or implied warranty. | This software is provided 'as-is', without any express or implied warranty. | ||||
| In no event will the authors be held liable for any damages arising from the use of this software. | In no event will the authors be held liable for any damages arising from the use of this software. | ||||
| Permission is granted to anyone to use this software for any purpose, | Permission is granted to anyone to use this software for any purpose, | ||||
| including commercial applications, and to alter it and redistribute it freely, | including commercial applications, and to alter it and redistribute it freely, | ||||
| subject to the following restrictions: | subject to the following restrictions: | ||||
| 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required. | ||||
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. | ||||
| 3. This notice may not be removed or altered from any source distribution. | 3. This notice may not be removed or altered from any source distribution. | ||||
| */ | */ | ||||
| #include "btGeometryUtil.h" | #include "btGeometryUtil.h" | ||||
| /* | /* | ||||
| Make sure this dummy function never changes so that it | Make sure this dummy function never changes so that it | ||||
| can be used by probes that are checking whether the | can be used by probes that are checking whether the | ||||
| library is actually installed. | library is actually installed. | ||||
| */ | */ | ||||
| extern "C" | extern "C" | ||||
| { | { | ||||
| void btBulletMathProbe (); | void btBulletMathProbe(); | ||||
| void btBulletMathProbe () {} | void btBulletMathProbe() {} | ||||
| } | } | ||||
| bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin) | bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin) | ||||
| { | { | ||||
| int numbrushes = planeEquations.size(); | int numbrushes = planeEquations.size(); | ||||
| for (int i=0;i<numbrushes;i++) | for (int i = 0; i < numbrushes; i++) | ||||
| { | { | ||||
| const btVector3& N1 = planeEquations[i]; | const btVector3& N1 = planeEquations[i]; | ||||
| btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin; | btScalar dist = btScalar(N1.dot(point)) + btScalar(N1[3]) - margin; | ||||
| if (dist>btScalar(0.)) | if (dist > btScalar(0.)) | ||||
| { | { | ||||
| return false; | return false; | ||||
| } | } | ||||
| } | } | ||||
| return true; | return true; | ||||
| } | } | ||||
| bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin) | bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin) | ||||
| { | { | ||||
| int numvertices = vertices.size(); | int numvertices = vertices.size(); | ||||
| for (int i=0;i<numvertices;i++) | for (int i = 0; i < numvertices; i++) | ||||
| { | { | ||||
| const btVector3& N1 = vertices[i]; | const btVector3& N1 = vertices[i]; | ||||
| btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin; | btScalar dist = btScalar(planeNormal.dot(N1)) + btScalar(planeNormal[3]) - margin; | ||||
| if (dist>btScalar(0.)) | if (dist > btScalar(0.)) | ||||
| { | { | ||||
| return false; | return false; | ||||
| } | } | ||||
| } | } | ||||
| return true; | return true; | ||||
| } | } | ||||
| bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations); | bool notExist(const btVector3& planeEquation, const btAlignedObjectArray<btVector3>& planeEquations); | ||||
| bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations) | bool notExist(const btVector3& planeEquation, const btAlignedObjectArray<btVector3>& planeEquations) | ||||
| { | { | ||||
| int numbrushes = planeEquations.size(); | int numbrushes = planeEquations.size(); | ||||
| for (int i=0;i<numbrushes;i++) | for (int i = 0; i < numbrushes; i++) | ||||
| { | { | ||||
| const btVector3& N1 = planeEquations[i]; | const btVector3& N1 = planeEquations[i]; | ||||
| if (planeEquation.dot(N1) > btScalar(0.999)) | if (planeEquation.dot(N1) > btScalar(0.999)) | ||||
| { | { | ||||
| return false; | return false; | ||||
| } | } | ||||
| } | } | ||||
| return true; | return true; | ||||
| } | } | ||||
| void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut ) | void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut) | ||||
| { | { | ||||
| const int numvertices = vertices.size(); | const int numvertices = vertices.size(); | ||||
| // brute force: | // brute force: | ||||
| for (int i=0;i<numvertices;i++) | for (int i = 0; i < numvertices; i++) | ||||
| { | { | ||||
| const btVector3& N1 = vertices[i]; | const btVector3& N1 = vertices[i]; | ||||
| for (int j=i+1;j<numvertices;j++) | for (int j = i + 1; j < numvertices; j++) | ||||
| { | { | ||||
| const btVector3& N2 = vertices[j]; | const btVector3& N2 = vertices[j]; | ||||
| for (int k=j+1;k<numvertices;k++) | for (int k = j + 1; k < numvertices; k++) | ||||
| { | { | ||||
| const btVector3& N3 = vertices[k]; | const btVector3& N3 = vertices[k]; | ||||
| btVector3 planeEquation,edge0,edge1; | btVector3 planeEquation, edge0, edge1; | ||||
| edge0 = N2-N1; | edge0 = N2 - N1; | ||||
| edge1 = N3-N1; | edge1 = N3 - N1; | ||||
| btScalar normalSign = btScalar(1.); | btScalar normalSign = btScalar(1.); | ||||
| for (int ww=0;ww<2;ww++) | for (int ww = 0; ww < 2; ww++) | ||||
| { | { | ||||
| planeEquation = normalSign * edge0.cross(edge1); | planeEquation = normalSign * edge0.cross(edge1); | ||||
| if (planeEquation.length2() > btScalar(0.0001)) | if (planeEquation.length2() > btScalar(0.0001)) | ||||
| { | { | ||||
| planeEquation.normalize(); | planeEquation.normalize(); | ||||
| if (notExist(planeEquation,planeEquationsOut)) | if (notExist(planeEquation, planeEquationsOut)) | ||||
| { | { | ||||
| planeEquation[3] = -planeEquation.dot(N1); | planeEquation[3] = -planeEquation.dot(N1); | ||||
| //check if inside, and replace supportingVertexOut if needed | //check if inside, and replace supportingVertexOut if needed | ||||
| if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01))) | if (areVerticesBehindPlane(planeEquation, vertices, btScalar(0.01))) | ||||
| { | { | ||||
| planeEquationsOut.push_back(planeEquation); | planeEquationsOut.push_back(planeEquation); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| normalSign = btScalar(-1.); | normalSign = btScalar(-1.); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut ) | void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations, btAlignedObjectArray<btVector3>& verticesOut) | ||||
| { | { | ||||
| const int numbrushes = planeEquations.size(); | const int numbrushes = planeEquations.size(); | ||||
| // brute force: | // brute force: | ||||
| for (int i=0;i<numbrushes;i++) | for (int i = 0; i < numbrushes; i++) | ||||
| { | { | ||||
| const btVector3& N1 = planeEquations[i]; | const btVector3& N1 = planeEquations[i]; | ||||
| for (int j=i+1;j<numbrushes;j++) | for (int j = i + 1; j < numbrushes; j++) | ||||
| { | { | ||||
| const btVector3& N2 = planeEquations[j]; | const btVector3& N2 = planeEquations[j]; | ||||
| for (int k=j+1;k<numbrushes;k++) | for (int k = j + 1; k < numbrushes; k++) | ||||
| { | { | ||||
| const btVector3& N3 = planeEquations[k]; | const btVector3& N3 = planeEquations[k]; | ||||
| btVector3 n2n3; n2n3 = N2.cross(N3); | btVector3 n2n3; | ||||
| btVector3 n3n1; n3n1 = N3.cross(N1); | n2n3 = N2.cross(N3); | ||||
| btVector3 n1n2; n1n2 = N1.cross(N2); | btVector3 n3n1; | ||||
| n3n1 = N3.cross(N1); | |||||
| btVector3 n1n2; | |||||
| n1n2 = N1.cross(N2); | |||||
| if ( ( n2n3.length2() > btScalar(0.0001) ) && | if ((n2n3.length2() > btScalar(0.0001)) && | ||||
| ( n3n1.length2() > btScalar(0.0001) ) && | (n3n1.length2() > btScalar(0.0001)) && | ||||
| ( n1n2.length2() > btScalar(0.0001) ) ) | (n1n2.length2() > btScalar(0.0001))) | ||||
| { | { | ||||
| //point P out of 3 plane equations: | //point P out of 3 plane equations: | ||||
| // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) | // d1 ( N2 * N3 ) + d2 ( N3 * N1 ) + d3 ( N1 * N2 ) | ||||
| //P = ------------------------------------------------------------------------- | //P = ------------------------------------------------------------------------- | ||||
| // N1 . ( N2 * N3 ) | // N1 . ( N2 * N3 ) | ||||
| btScalar quotient = (N1.dot(n2n3)); | btScalar quotient = (N1.dot(n2n3)); | ||||
| if (btFabs(quotient) > btScalar(0.000001)) | if (btFabs(quotient) > btScalar(0.000001)) | ||||
| { | { | ||||
| quotient = btScalar(-1.) / quotient; | quotient = btScalar(-1.) / quotient; | ||||
| n2n3 *= N1[3]; | n2n3 *= N1[3]; | ||||
| n3n1 *= N2[3]; | n3n1 *= N2[3]; | ||||
| n1n2 *= N3[3]; | n1n2 *= N3[3]; | ||||
| btVector3 potentialVertex = n2n3; | btVector3 potentialVertex = n2n3; | ||||
| potentialVertex += n3n1; | potentialVertex += n3n1; | ||||
| potentialVertex += n1n2; | potentialVertex += n1n2; | ||||
| potentialVertex *= quotient; | potentialVertex *= quotient; | ||||
| //check if inside, and replace supportingVertexOut if needed | //check if inside, and replace supportingVertexOut if needed | ||||
| if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01))) | if (isPointInsidePlanes(planeEquations, potentialVertex, btScalar(0.01))) | ||||
| { | { | ||||
| verticesOut.push_back(potentialVertex); | verticesOut.push_back(potentialVertex); | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||
| } | } | ||||