基于osg的obb包围盒计算

#include "stdafx.h" #include "OBBbox.hpp" #define ROTATE(a,i,j,k,l) g=a(i,j); h=a(k, l); a(i, j)=(float)(g-s*(h+g*tau)); a(k, l)=(float)(h+s*(g-h*tau)); namespace render_system { static osg::Matrix transpose(const osg::Matrix& b) { osg::Matrix mat; //mat.inverse(b); //mat.invert(b); for (int i = 0; i < 4; ++i) { for (int j = 0; j < 4; ++j) { mat(i, j) = b(j, i); } } return mat; } osg::ref_ptr<osg::Vec3Array> getPointsPolarCorner(const osg::ref_ptr<osg::Vec3Array>& vertPos, osg::Vec3& ptStart, osg::Vec3& ptEnd) { osg::ref_ptr<osg::Vec3Array> ptPollar = new osg::Vec3Array; if (!vertPos.valid() || vertPos->size() < 1) { return ptPollar; } //0、1 x: max/min //2、3 y: max/min //4、5 z: max/min ptPollar->push_back(vertPos->at(0)); ptPollar->push_back(vertPos->at(0)); ptPollar->push_back(vertPos->at(0)); ptPollar->push_back(vertPos->at(0)); ptPollar->push_back(vertPos->at(0)); ptPollar->push_back(vertPos->at(0)); for (osg::Vec3Array::size_type i=1; i<vertPos->size(); i++) { osg::Vec3& ptcur = vertPos->at(i); if (ptcur.x() > ptPollar->at(0).x()) ptPollar->at(0) = ptcur; if (ptcur.x() < ptPollar->at(1).x()) ptPollar->at(1) = ptcur; if (ptcur.y() > ptPollar->at(2).y()) ptPollar->at(2) = ptcur; if (ptcur.y() < ptPollar->at(3).y()) ptPollar->at(3) = ptcur; if (ptcur.z() > ptPollar->at(4).z()) ptPollar->at(4) = ptcur; if (ptcur.z() < ptPollar->at(5).z()) ptPollar->at(5) = ptcur; } double dDis = 0.0; ptStart = ptPollar->at(0); ptEnd = ptStart; for (int i=0; i<6; i++) { for (int j=0; j<6; j++) { float ssss = (ptPollar->at(i)-ptPollar->at(j)).length(); if ( ssss > dDis) { dDis = ssss; ptStart = ptPollar->at(i); ptEnd = ptPollar->at(j); } } } return ptPollar; } //中心点，以对角线为方向的平面 void calcPlaneEquation(const osg::Vec3& ptStart, const osg::Vec3& ptEnd, float& A, float& B,float& C, float& D) { osg::Vec3 ptCenter = (ptEnd + ptStart)/2.0; osg::Vec3 vDir = ptEnd - ptStart; vDir.normalize(); A = vDir.x(); B = vDir.y(); C = vDir.z(); D = -(ptCenter.x()*A + ptCenter.y()*B + ptCenter.z()*C); } //点到平面上的投影 osg::Vec3 pointProjectToPlane(const osg::Vec3 &pt, float A, float B,float C, float D) { float t = (A*pt.x() + B*pt.y() + C*pt.z() + D)/(A*A + B*B + C*C); return osg::Vec3(pt.x()-A*t, pt.y()-B*t, pt.z()-C*t); } //点到线的投影 osg::Vec3 pointProjectToLine(const osg::Vec3 &pt, const osg::Vec3 &ptonline, const osg::Vec3 &linedir) { osg::Vec3 retpt; float cosangl = (pt - ptonline) * linedir; retpt = ptonline + (linedir * cosangl); return retpt; } //一组点的aabb极点 bool getRangePoint(const osg::ref_ptr<osg::Vec3Array>& vertPos, osg::Vec3& ptmax, osg::Vec3& ptmin) { if (!vertPos.valid() || vertPos->size() < 1) { return false; } ptmax = ptmin = vertPos->at(0); for (osg::Vec3Array::size_type i=1; i<vertPos->size(); ++i) { if (vertPos->at(i).x() > ptmax.x()) ptmax.x() = vertPos->at(i).x(); if (vertPos->at(i).x() > ptmax.x()) ptmax.x() = vertPos->at(i).x(); if (vertPos->at(i).y() > ptmax.y()) ptmax.y() = vertPos->at(i).y(); if (vertPos->at(i).y() > ptmax.y()) ptmax.y() = vertPos->at(i).y(); if (vertPos->at(i).z() > ptmax.z()) ptmax.z() = vertPos->at(i).z(); if (vertPos->at(i).z() > ptmax.z()) ptmax.z() = vertPos->at(i).z(); } return true; } osg::ref_ptr<osg::Vec3Array> GetOBBCorner(osg::ref_ptr<osg::Vec3Array>& vertPos) { osg::ref_ptr<osg::Vec3Array> retArr = new osg::Vec3Array; osg::Vec3 ptStart, ptEnd; osg::ref_ptr<osg::Vec3Array> ptPollar; //std::vector<osg::Vec3> ptPollar; ptPollar = getPointsPolarCorner(vertPos, ptStart, ptEnd); if (ptPollar->size() < 1) { return retArr; } float A, B, C, D; A= B = C = D = 0.0; calcPlaneEquation(ptStart, ptEnd, A, B, C, D); osg::ref_ptr<osg::Vec3Array> projectpts = new osg::Vec3Array;; for (osg::Vec3Array::size_type i = 0; i < ptPollar->size(); i++) { projectpts->push_back(pointProjectToPlane(ptPollar->at(i), A, B, C, D)); } osg::Vec3 vHlfDir = (ptEnd - ptStart)/2.0; osg::Vec3 ptymax, ptymin; getPointsPolarCorner(projectpts, ptymax, ptymin); osg::Vec3 vnor = ptymax - ptymin; osg::Vec3 ptycenter = (ptymax+ptymin)/2.0; vnor = vnor^vHlfDir; vnor.normalize(); //calcPlaneEquation(ptymax, ptymin, A, B, C, D); osg::ref_ptr<osg::Vec3Array> project2d1 = new osg::Vec3Array;; for (osg::Vec3Array::size_type i = 0; i < projectpts->size(); i++) { project2d1->push_back(pointProjectToLine(projectpts->at(i), ptycenter, vnor)); } osg::Vec3 ptzmax, ptzmin; getPointsPolarCorner(project2d1, ptzmax, ptzmin); osg::Vec3 ptzcenter = (ptzmax+ptzmin)/2.0; retArr->push_back(ptymax + ptzcenter - ptzmax + vHlfDir); retArr->push_back(ptymin + ptzcenter - ptzmax + vHlfDir); retArr->push_back(ptzmax + ptycenter - ptymax + vHlfDir); retArr->push_back(ptymax + ptzmax - ptzcenter + vHlfDir); retArr->push_back(ptymax + ptzmax - ptzcenter - vHlfDir); retArr->push_back(ptzmax + ptycenter - ptymax - vHlfDir); retArr->push_back(ptymin + ptzcenter - ptzmax - vHlfDir); retArr->push_back(ptymax + ptzcenter - ptzmax - vHlfDir); // retArr->push_back(ptymin - vHlfDir); // retArr->push_back(ptzmin - vHlfDir); // retArr->push_back(ptymax - vHlfDir); // retArr->push_back(ptzmax - vHlfDir); return retArr; } static osg::Matrix _getConvarianceMatrix(const osg::ref_ptr<osg::Vec3Array> vertPos) { int i; osg::Matrix Cov; double S1[3]; double S2[3][3]; S1[0] = S1[1] = S1[2] = 0.0; S2[0][0] = S2[1][0] = S2[2][0] = 0.0; S2[0][1] = S2[1][1] = S2[2][1] = 0.0; S2[0][2] = S2[1][2] = S2[2][2] = 0.0; // get center of mass int vertCount = vertPos->size(); for (i = 0; i < vertCount; i++) { S1[0] += (*vertPos)[i].x(); S1[1] += (*vertPos)[i].y(); S1[2] += (*vertPos)[i].z(); S2[0][0] += (*vertPos)[i].x() * (*vertPos)[i].x(); S2[1][1] += (*vertPos)[i].y() * (*vertPos)[i].y(); S2[2][2] += (*vertPos)[i].z() * (*vertPos)[i].z(); S2[0][1] += (*vertPos)[i].x() * (*vertPos)[i].y(); S2[0][2] += (*vertPos)[i].x() * (*vertPos)[i].z(); S2[1][2] += (*vertPos)[i].y() * (*vertPos)[i].z(); } float n = (float)vertCount; // now get covariances Cov(0, 0) = (float)(S2[0][0] - S1[0] * S1[0] / n) / n; Cov(1, 1) = (float)(S2[1][1] - S1[1] * S1[1] / n) / n; Cov(2, 2) = (float)(S2[2][2] - S1[2] * S1[2] / n) / n; Cov(0, 1) = (float)(S2[0][1] - S1[0] * S1[1] / n) / n; Cov(1, 2) = (float)(S2[1][2] - S1[1] * S1[2] / n) / n; Cov(0, 2) = (float)(S2[0][2] - S1[0] * S1[2] / n) / n; Cov(1, 0) = Cov(0, 1); Cov(2, 0) = Cov(0, 2); Cov(2, 1) = Cov(1, 2); return Cov; } static float& _getElement(osg::Vec3& point, int index) { if (index == 0) return point.x(); if (index == 1) return point.y(); if (index == 2) return point.z(); return point.x(); } static void _getEigenVectors(osg::Matrix* vout, osg::Vec3* dout, osg::Matrix a) { int n = 3; int j, iq, ip, i; double tresh, theta, tau, t, sm, s, h, g, c; int nrot; osg::Vec3 b; osg::Vec3 z; osg::Matrix v; osg::Vec3 d; v = osg::Matrix::identity(); for (ip = 0; ip < n; ip++) { _getElement(b, ip) = a(ip, ip);// a.m[ip + 4 * ip]; _getElement(d, ip) = a(ip, ip);;// a.m[ip + 4 * ip]; _getElement(z, ip) = 0.0; } nrot = 0; for (i = 0; i < 50; i++) { sm = 0.0; for (ip = 0; ip < n; ip++) for (iq = ip + 1; iq < n; iq++) sm += fabs((a(ip ,iq))); if (false/*fabs(sm) < FLT_EPSILON*/) { v = transpose(v); //v.transpose(); *vout = v; *dout = d; return; } if (i < 3