#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
- 1
- 2
前往页