Go to the documentation of this file.00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017 #include "btGeometryUtil.h"
00018
00019
00020
00021
00022
00023
00024
00025 extern "C"
00026 {
00027 void btBulletMathProbe ();
00028
00029 void btBulletMathProbe () {}
00030 }
00031
00032
00033 bool btGeometryUtil::isPointInsidePlanes(const btAlignedObjectArray<btVector3>& planeEquations, const btVector3& point, btScalar margin)
00034 {
00035 int numbrushes = planeEquations.size();
00036 for (int i=0;i<numbrushes;i++)
00037 {
00038 const btVector3& N1 = planeEquations[i];
00039 btScalar dist = btScalar(N1.dot(point))+btScalar(N1[3])-margin;
00040 if (dist>btScalar(0.))
00041 {
00042 return false;
00043 }
00044 }
00045 return true;
00046
00047 }
00048
00049
00050 bool btGeometryUtil::areVerticesBehindPlane(const btVector3& planeNormal, const btAlignedObjectArray<btVector3>& vertices, btScalar margin)
00051 {
00052 int numvertices = vertices.size();
00053 for (int i=0;i<numvertices;i++)
00054 {
00055 const btVector3& N1 = vertices[i];
00056 btScalar dist = btScalar(planeNormal.dot(N1))+btScalar(planeNormal[3])-margin;
00057 if (dist>btScalar(0.))
00058 {
00059 return false;
00060 }
00061 }
00062 return true;
00063 }
00064
00065 bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations);
00066
00067 bool notExist(const btVector3& planeEquation,const btAlignedObjectArray<btVector3>& planeEquations)
00068 {
00069 int numbrushes = planeEquations.size();
00070 for (int i=0;i<numbrushes;i++)
00071 {
00072 const btVector3& N1 = planeEquations[i];
00073 if (planeEquation.dot(N1) > btScalar(0.999))
00074 {
00075 return false;
00076 }
00077 }
00078 return true;
00079 }
00080
00081 void btGeometryUtil::getPlaneEquationsFromVertices(btAlignedObjectArray<btVector3>& vertices, btAlignedObjectArray<btVector3>& planeEquationsOut )
00082 {
00083 const int numvertices = vertices.size();
00084
00085 for (int i=0;i<numvertices;i++)
00086 {
00087 const btVector3& N1 = vertices[i];
00088
00089
00090 for (int j=i+1;j<numvertices;j++)
00091 {
00092 const btVector3& N2 = vertices[j];
00093
00094 for (int k=j+1;k<numvertices;k++)
00095 {
00096
00097 const btVector3& N3 = vertices[k];
00098
00099 btVector3 planeEquation,edge0,edge1;
00100 edge0 = N2-N1;
00101 edge1 = N3-N1;
00102 btScalar normalSign = btScalar(1.);
00103 for (int ww=0;ww<2;ww++)
00104 {
00105 planeEquation = normalSign * edge0.cross(edge1);
00106 if (planeEquation.length2() > btScalar(0.0001))
00107 {
00108 planeEquation.normalize();
00109 if (notExist(planeEquation,planeEquationsOut))
00110 {
00111 planeEquation[3] = -planeEquation.dot(N1);
00112
00113
00114 if (areVerticesBehindPlane(planeEquation,vertices,btScalar(0.01)))
00115 {
00116 planeEquationsOut.push_back(planeEquation);
00117 }
00118 }
00119 }
00120 normalSign = btScalar(-1.);
00121 }
00122
00123 }
00124 }
00125 }
00126
00127 }
00128
00129 void btGeometryUtil::getVerticesFromPlaneEquations(const btAlignedObjectArray<btVector3>& planeEquations , btAlignedObjectArray<btVector3>& verticesOut )
00130 {
00131 const int numbrushes = planeEquations.size();
00132
00133 for (int i=0;i<numbrushes;i++)
00134 {
00135 const btVector3& N1 = planeEquations[i];
00136
00137
00138 for (int j=i+1;j<numbrushes;j++)
00139 {
00140 const btVector3& N2 = planeEquations[j];
00141
00142 for (int k=j+1;k<numbrushes;k++)
00143 {
00144
00145 const btVector3& N3 = planeEquations[k];
00146
00147 btVector3 n2n3; n2n3 = N2.cross(N3);
00148 btVector3 n3n1; n3n1 = N3.cross(N1);
00149 btVector3 n1n2; n1n2 = N1.cross(N2);
00150
00151 if ( ( n2n3.length2() > btScalar(0.0001) ) &&
00152 ( n3n1.length2() > btScalar(0.0001) ) &&
00153 ( n1n2.length2() > btScalar(0.0001) ) )
00154 {
00155
00156
00157
00158
00159
00160
00161
00162 btScalar quotient = (N1.dot(n2n3));
00163 if (btFabs(quotient) > btScalar(0.000001))
00164 {
00165 quotient = btScalar(-1.) / quotient;
00166 n2n3 *= N1[3];
00167 n3n1 *= N2[3];
00168 n1n2 *= N3[3];
00169 btVector3 potentialVertex = n2n3;
00170 potentialVertex += n3n1;
00171 potentialVertex += n1n2;
00172 potentialVertex *= quotient;
00173
00174
00175 if (isPointInsidePlanes(planeEquations,potentialVertex,btScalar(0.01)))
00176 {
00177 verticesOut.push_back(potentialVertex);
00178 }
00179 }
00180 }
00181 }
00182 }
00183 }
00184 }
00185