mirror of
https://github.com/AxioDL/metaforce.git
synced 2025-12-09 01:07:43 +00:00
New code style refactor
This commit is contained in:
Jack Andersen
@@ -5,86 +5,80 @@
|
||||
#include "gmm/gmm.h"
|
||||
#include "hecl/Blender/Connection.hpp"
|
||||
|
||||
namespace DataSpec
|
||||
{
|
||||
namespace DataSpec {
|
||||
|
||||
using ColMesh = hecl::blender::ColMesh;
|
||||
|
||||
struct FittedOBB
|
||||
{
|
||||
zeus::CTransform xf;
|
||||
zeus::CVector3f he;
|
||||
struct FittedOBB {
|
||||
zeus::CTransform xf;
|
||||
zeus::CVector3f he;
|
||||
};
|
||||
|
||||
static std::vector<int> MakeRootTriangleIndex(const ColMesh& mesh)
|
||||
{
|
||||
std::vector<int> ret;
|
||||
ret.reserve(mesh.trianges.size());
|
||||
for (int i = 0; i < mesh.trianges.size(); ++i)
|
||||
ret.push_back(i);
|
||||
return ret;
|
||||
static std::vector<int> MakeRootTriangleIndex(const ColMesh& mesh) {
|
||||
std::vector<int> ret;
|
||||
ret.reserve(mesh.trianges.size());
|
||||
for (int i = 0; i < mesh.trianges.size(); ++i)
|
||||
ret.push_back(i);
|
||||
return ret;
|
||||
}
|
||||
|
||||
static std::unordered_set<uint32_t> GetTriangleVerts(const ColMesh& mesh, int triIdx)
|
||||
{
|
||||
const ColMesh::Triangle& T = mesh.trianges[triIdx];
|
||||
std::unordered_set<uint32_t> verts;
|
||||
verts.insert(mesh.edges[T.edges[0]].verts[0]);
|
||||
verts.insert(mesh.edges[T.edges[0]].verts[1]);
|
||||
verts.insert(mesh.edges[T.edges[1]].verts[0]);
|
||||
verts.insert(mesh.edges[T.edges[1]].verts[1]);
|
||||
verts.insert(mesh.edges[T.edges[2]].verts[0]);
|
||||
verts.insert(mesh.edges[T.edges[2]].verts[1]);
|
||||
return verts;
|
||||
static std::unordered_set<uint32_t> GetTriangleVerts(const ColMesh& mesh, int triIdx) {
|
||||
const ColMesh::Triangle& T = mesh.trianges[triIdx];
|
||||
std::unordered_set<uint32_t> verts;
|
||||
verts.insert(mesh.edges[T.edges[0]].verts[0]);
|
||||
verts.insert(mesh.edges[T.edges[0]].verts[1]);
|
||||
verts.insert(mesh.edges[T.edges[1]].verts[0]);
|
||||
verts.insert(mesh.edges[T.edges[1]].verts[1]);
|
||||
verts.insert(mesh.edges[T.edges[2]].verts[0]);
|
||||
verts.insert(mesh.edges[T.edges[2]].verts[1]);
|
||||
return verts;
|
||||
}
|
||||
|
||||
// method to set the OBB parameters which produce a box oriented according to
|
||||
// the covariance matrix C, which just containts the points pnts
|
||||
static FittedOBB BuildFromCovarianceMatrix(gmm::dense_matrix<float>& C,
|
||||
const ColMesh& mesh, const std::vector<int>& index)
|
||||
{
|
||||
FittedOBB ret;
|
||||
static FittedOBB BuildFromCovarianceMatrix(gmm::dense_matrix<float>& C, const ColMesh& mesh,
|
||||
const std::vector<int>& index) {
|
||||
FittedOBB ret;
|
||||
|
||||
// extract the eigenvalues and eigenvectors from C
|
||||
gmm::dense_matrix<float> eigvec(3,3);
|
||||
std::vector<float> eigval(3);
|
||||
using namespace gmm;
|
||||
using MAT1 = gmm::dense_matrix<float>;
|
||||
gmm::symmetric_qr_algorithm(C, eigval, eigvec, default_tol_for_qr);
|
||||
// extract the eigenvalues and eigenvectors from C
|
||||
gmm::dense_matrix<float> eigvec(3, 3);
|
||||
std::vector<float> eigval(3);
|
||||
using namespace gmm;
|
||||
using MAT1 = gmm::dense_matrix<float>;
|
||||
gmm::symmetric_qr_algorithm(C, eigval, eigvec, default_tol_for_qr);
|
||||
|
||||
// find the right, up and forward vectors from the eigenvectors
|
||||
zeus::CVector3f r(eigvec(0,0), eigvec(1,0), eigvec(2,0));
|
||||
zeus::CVector3f u(eigvec(0,1), eigvec(1,1), eigvec(2,1));
|
||||
zeus::CVector3f f(eigvec(0,2), eigvec(1,2), eigvec(2,2));
|
||||
r.normalize(); u.normalize(), f.normalize();
|
||||
// find the right, up and forward vectors from the eigenvectors
|
||||
zeus::CVector3f r(eigvec(0, 0), eigvec(1, 0), eigvec(2, 0));
|
||||
zeus::CVector3f u(eigvec(0, 1), eigvec(1, 1), eigvec(2, 1));
|
||||
zeus::CVector3f f(eigvec(0, 2), eigvec(1, 2), eigvec(2, 2));
|
||||
r.normalize();
|
||||
u.normalize(), f.normalize();
|
||||
|
||||
// set the rotation matrix using the eigvenvectors
|
||||
ret.xf.basis[0] = r;
|
||||
ret.xf.basis[1] = u;
|
||||
ret.xf.basis[2] = f;
|
||||
// set the rotation matrix using the eigvenvectors
|
||||
ret.xf.basis[0] = r;
|
||||
ret.xf.basis[1] = u;
|
||||
ret.xf.basis[2] = f;
|
||||
|
||||
// now build the bounding box extents in the rotated frame
|
||||
zeus::CVector3f minim(1e10f, 1e10f, 1e10f), maxim(-1e10f, -1e10f, -1e10f);
|
||||
for (int triIdx : index)
|
||||
{
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, triIdx);
|
||||
for (uint32_t v : verts)
|
||||
{
|
||||
const zeus::CVector3f& p = mesh.verts[v].val;
|
||||
zeus::CVector3f p_prime(r.dot(p), u.dot(p), f.dot(p));
|
||||
minim = zeus::min(minim, p_prime);
|
||||
maxim = zeus::max(maxim, p_prime);
|
||||
}
|
||||
// now build the bounding box extents in the rotated frame
|
||||
zeus::CVector3f minim(1e10f, 1e10f, 1e10f), maxim(-1e10f, -1e10f, -1e10f);
|
||||
for (int triIdx : index) {
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, triIdx);
|
||||
for (uint32_t v : verts) {
|
||||
const zeus::CVector3f& p = mesh.verts[v].val;
|
||||
zeus::CVector3f p_prime(r.dot(p), u.dot(p), f.dot(p));
|
||||
minim = zeus::min(minim, p_prime);
|
||||
maxim = zeus::max(maxim, p_prime);
|
||||
}
|
||||
}
|
||||
|
||||
// set the center of the OBB to be the average of the
|
||||
// minimum and maximum, and the extents be half of the
|
||||
// difference between the minimum and maximum
|
||||
zeus::CVector3f center = (maxim + minim) * 0.5f;
|
||||
ret.xf.origin = ret.xf.basis * center;
|
||||
ret.he = (maxim - minim) * 0.5f;
|
||||
// set the center of the OBB to be the average of the
|
||||
// minimum and maximum, and the extents be half of the
|
||||
// difference between the minimum and maximum
|
||||
zeus::CVector3f center = (maxim + minim) * 0.5f;
|
||||
ret.xf.origin = ret.xf.basis * center;
|
||||
ret.he = (maxim - minim) * 0.5f;
|
||||
|
||||
return ret;
|
||||
return ret;
|
||||
}
|
||||
|
||||
// builds an OBB from triangles specified as an array of
|
||||
@@ -93,166 +87,164 @@ static FittedOBB BuildFromCovarianceMatrix(gmm::dense_matrix<float>& C,
|
||||
// method build_from_covariance_matrix() method to fit
|
||||
// the box. ALL points will be fit in the box, regardless
|
||||
// of whether they are indexed by a triangle or not.
|
||||
static FittedOBB FitOBB(const ColMesh& mesh, const std::vector<int>& index)
|
||||
{
|
||||
float Ai, Am=0.0;
|
||||
zeus::CVector3f mu, mui;
|
||||
gmm::dense_matrix<float> C(3,3);
|
||||
float cxx=0.0, cxy=0.0, cxz=0.0, cyy=0.0, cyz=0.0, czz=0.0;
|
||||
static FittedOBB FitOBB(const ColMesh& mesh, const std::vector<int>& index) {
|
||||
float Ai, Am = 0.0;
|
||||
zeus::CVector3f mu, mui;
|
||||
gmm::dense_matrix<float> C(3, 3);
|
||||
float cxx = 0.0, cxy = 0.0, cxz = 0.0, cyy = 0.0, cyz = 0.0, czz = 0.0;
|
||||
|
||||
// loop over the triangles this time to find the
|
||||
// mean location
|
||||
for (int i : index)
|
||||
{
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, i);
|
||||
auto it = verts.begin();
|
||||
zeus::CVector3f p = mesh.verts[*it++].val;
|
||||
zeus::CVector3f q = mesh.verts[*it++].val;
|
||||
zeus::CVector3f r = mesh.verts[*it++].val;
|
||||
mui = (p+q+r)/3.f;
|
||||
Ai = (q-p).cross(r-p).magnitude() / 2.f;
|
||||
mu += mui*Ai;
|
||||
Am += Ai;
|
||||
// loop over the triangles this time to find the
|
||||
// mean location
|
||||
for (int i : index) {
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, i);
|
||||
auto it = verts.begin();
|
||||
zeus::CVector3f p = mesh.verts[*it++].val;
|
||||
zeus::CVector3f q = mesh.verts[*it++].val;
|
||||
zeus::CVector3f r = mesh.verts[*it++].val;
|
||||
mui = (p + q + r) / 3.f;
|
||||
Ai = (q - p).cross(r - p).magnitude() / 2.f;
|
||||
mu += mui * Ai;
|
||||
Am += Ai;
|
||||
|
||||
// these bits set the c terms to Am*E[xx], Am*E[xy], Am*E[xz]....
|
||||
cxx += ( 9.0*mui.x()*mui.x() + p.x()*p.x() + q.x()*q.x() + r.x()*r.x() )*(Ai/12.0);
|
||||
cxy += ( 9.0*mui.x()*mui.y() + p.x()*p.y() + q.x()*q.y() + r.x()*r.y() )*(Ai/12.0);
|
||||
cxz += ( 9.0*mui.x()*mui.z() + p.x()*p.z() + q.x()*q.z() + r.x()*r.z() )*(Ai/12.0);
|
||||
cyy += ( 9.0*mui.y()*mui.y() + p.y()*p.y() + q.y()*q.y() + r.y()*r.y() )*(Ai/12.0);
|
||||
cyz += ( 9.0*mui.y()*mui.z() + p.y()*p.z() + q.y()*q.z() + r.y()*r.z() )*(Ai/12.0);
|
||||
}
|
||||
// these bits set the c terms to Am*E[xx], Am*E[xy], Am*E[xz]....
|
||||
cxx += (9.0 * mui.x() * mui.x() + p.x() * p.x() + q.x() * q.x() + r.x() * r.x()) * (Ai / 12.0);
|
||||
cxy += (9.0 * mui.x() * mui.y() + p.x() * p.y() + q.x() * q.y() + r.x() * r.y()) * (Ai / 12.0);
|
||||
cxz += (9.0 * mui.x() * mui.z() + p.x() * p.z() + q.x() * q.z() + r.x() * r.z()) * (Ai / 12.0);
|
||||
cyy += (9.0 * mui.y() * mui.y() + p.y() * p.y() + q.y() * q.y() + r.y() * r.y()) * (Ai / 12.0);
|
||||
cyz += (9.0 * mui.y() * mui.z() + p.y() * p.z() + q.y() * q.z() + r.y() * r.z()) * (Ai / 12.0);
|
||||
}
|
||||
|
||||
if (zeus::close_enough(Am, 0.f))
|
||||
return {};
|
||||
if (zeus::close_enough(Am, 0.f))
|
||||
return {};
|
||||
|
||||
// divide out the Am fraction from the average position and
|
||||
// covariance terms
|
||||
mu = mu / Am;
|
||||
cxx /= Am; cxy /= Am; cxz /= Am; cyy /= Am; cyz /= Am; czz /= Am;
|
||||
// divide out the Am fraction from the average position and
|
||||
// covariance terms
|
||||
mu = mu / Am;
|
||||
cxx /= Am;
|
||||
cxy /= Am;
|
||||
cxz /= Am;
|
||||
cyy /= Am;
|
||||
cyz /= Am;
|
||||
czz /= Am;
|
||||
|
||||
// now subtract off the E[x]*E[x], E[x]*E[y], ... terms
|
||||
cxx -= mu.x()*mu.x(); cxy -= mu.x()*mu.y(); cxz -= mu.x()*mu.z();
|
||||
cyy -= mu.y()*mu.y(); cyz -= mu.y()*mu.z(); czz -= mu.z()*mu.z();
|
||||
// now subtract off the E[x]*E[x], E[x]*E[y], ... terms
|
||||
cxx -= mu.x() * mu.x();
|
||||
cxy -= mu.x() * mu.y();
|
||||
cxz -= mu.x() * mu.z();
|
||||
cyy -= mu.y() * mu.y();
|
||||
cyz -= mu.y() * mu.z();
|
||||
czz -= mu.z() * mu.z();
|
||||
|
||||
// now build the covariance matrix
|
||||
C(0,0)=cxx; C(0,1)=cxy; C(0,2)=cxz;
|
||||
C(1,0)=cxy; C(1,1)=cyy; C(1,2)=cyz;
|
||||
C(2,0)=cxz; C(2,1)=cyz; C(2,2)=czz;
|
||||
// now build the covariance matrix
|
||||
C(0, 0) = cxx;
|
||||
C(0, 1) = cxy;
|
||||
C(0, 2) = cxz;
|
||||
C(1, 0) = cxy;
|
||||
C(1, 1) = cyy;
|
||||
C(1, 2) = cyz;
|
||||
C(2, 0) = cxz;
|
||||
C(2, 1) = cyz;
|
||||
C(2, 2) = czz;
|
||||
|
||||
// set the obb parameters from the covariance matrix
|
||||
return BuildFromCovarianceMatrix(C, mesh, index);
|
||||
// set the obb parameters from the covariance matrix
|
||||
return BuildFromCovarianceMatrix(C, mesh, index);
|
||||
}
|
||||
|
||||
template <typename Node>
|
||||
static void MakeLeaf(const ColMesh& mesh, const std::vector<int>& index, Node& n)
|
||||
{
|
||||
n.left.reset();
|
||||
n.right.reset();
|
||||
n.isLeaf = true;
|
||||
n.leafData = std::make_unique<typename Node::LeafData>();
|
||||
n.leafData->triangleIndexCount = atUint32(index.size());
|
||||
n.leafData->triangleIndices.reserve(n.leafData->triangleIndexCount);
|
||||
for (int i : index)
|
||||
n.leafData->triangleIndices.push_back(i);
|
||||
static void MakeLeaf(const ColMesh& mesh, const std::vector<int>& index, Node& n) {
|
||||
n.left.reset();
|
||||
n.right.reset();
|
||||
n.isLeaf = true;
|
||||
n.leafData = std::make_unique<typename Node::LeafData>();
|
||||
n.leafData->triangleIndexCount = atUint32(index.size());
|
||||
n.leafData->triangleIndices.reserve(n.leafData->triangleIndexCount);
|
||||
for (int i : index)
|
||||
n.leafData->triangleIndices.push_back(i);
|
||||
}
|
||||
|
||||
template <typename Node>
|
||||
static std::unique_ptr<Node> RecursiveMakeNode(const ColMesh& mesh, const std::vector<int>& index)
|
||||
{
|
||||
// calculate root OBB
|
||||
FittedOBB obb = FitOBB(mesh, index);
|
||||
static std::unique_ptr<Node> RecursiveMakeNode(const ColMesh& mesh, const std::vector<int>& index) {
|
||||
// calculate root OBB
|
||||
FittedOBB obb = FitOBB(mesh, index);
|
||||
|
||||
// make results row-major and also invert the rotation basis
|
||||
obb.xf.basis.transpose();
|
||||
// make results row-major and also invert the rotation basis
|
||||
obb.xf.basis.transpose();
|
||||
|
||||
std::unique_ptr<Node> n = std::make_unique<Node>();
|
||||
for (int i = 0; i < 3; ++i)
|
||||
{
|
||||
n->xf[i] = zeus::CVector4f{obb.xf.basis[i]};
|
||||
n->xf[i].simd[3] = float(obb.xf.origin[i]);
|
||||
}
|
||||
n->halfExtent = obb.he;
|
||||
|
||||
// terminate branch when volume < 1.0
|
||||
if (obb.he[0] * obb.he[1] * obb.he[2] < 1.f)
|
||||
{
|
||||
MakeLeaf(mesh, index, *n);
|
||||
return n;
|
||||
}
|
||||
|
||||
n->isLeaf = false;
|
||||
|
||||
std::vector<int> indexNeg[3];
|
||||
std::vector<int> indexPos[3];
|
||||
for (int c = 0; c < 3; ++c)
|
||||
{
|
||||
// subdivide negative side
|
||||
indexNeg[c].reserve(index.size());
|
||||
for (int i : index)
|
||||
{
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, i);
|
||||
for (uint32_t vtx : verts)
|
||||
{
|
||||
zeus::CVector3f v = mesh.verts[vtx].val;
|
||||
v = obb.xf.basis * (v - obb.xf.origin);
|
||||
if (v[c] < 0.f)
|
||||
{
|
||||
indexNeg[c].push_back(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// subdivide positive side
|
||||
indexPos[c].reserve(index.size());
|
||||
for (int i : index)
|
||||
{
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, i);
|
||||
for (uint32_t vtx : verts)
|
||||
{
|
||||
zeus::CVector3f v = mesh.verts[vtx].val;
|
||||
v = obb.xf.basis * (v - obb.xf.origin);
|
||||
if (v[c] >= 0.f)
|
||||
{
|
||||
indexPos[c].push_back(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
size_t idxMin = index.size();
|
||||
int minComp = -1;
|
||||
for (int c = 0; c < 3; ++c)
|
||||
{
|
||||
size_t test = std::max(indexNeg[c].size(), indexPos[c].size());
|
||||
if (test < idxMin && test < index.size() * 3 / 4)
|
||||
{
|
||||
minComp = c;
|
||||
idxMin = test;
|
||||
}
|
||||
}
|
||||
|
||||
if (minComp == -1)
|
||||
{
|
||||
MakeLeaf(mesh, index, *n);
|
||||
return n;
|
||||
}
|
||||
|
||||
n->left = RecursiveMakeNode<Node>(mesh, indexNeg[minComp]);
|
||||
n->right = RecursiveMakeNode<Node>(mesh, indexPos[minComp]);
|
||||
std::unique_ptr<Node> n = std::make_unique<Node>();
|
||||
for (int i = 0; i < 3; ++i) {
|
||||
n->xf[i] = zeus::CVector4f{obb.xf.basis[i]};
|
||||
n->xf[i].simd[3] = float(obb.xf.origin[i]);
|
||||
}
|
||||
n->halfExtent = obb.he;
|
||||
|
||||
// terminate branch when volume < 1.0
|
||||
if (obb.he[0] * obb.he[1] * obb.he[2] < 1.f) {
|
||||
MakeLeaf(mesh, index, *n);
|
||||
return n;
|
||||
}
|
||||
|
||||
n->isLeaf = false;
|
||||
|
||||
std::vector<int> indexNeg[3];
|
||||
std::vector<int> indexPos[3];
|
||||
for (int c = 0; c < 3; ++c) {
|
||||
// subdivide negative side
|
||||
indexNeg[c].reserve(index.size());
|
||||
for (int i : index) {
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, i);
|
||||
for (uint32_t vtx : verts) {
|
||||
zeus::CVector3f v = mesh.verts[vtx].val;
|
||||
v = obb.xf.basis * (v - obb.xf.origin);
|
||||
if (v[c] < 0.f) {
|
||||
indexNeg[c].push_back(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// subdivide positive side
|
||||
indexPos[c].reserve(index.size());
|
||||
for (int i : index) {
|
||||
std::unordered_set<uint32_t> verts = GetTriangleVerts(mesh, i);
|
||||
for (uint32_t vtx : verts) {
|
||||
zeus::CVector3f v = mesh.verts[vtx].val;
|
||||
v = obb.xf.basis * (v - obb.xf.origin);
|
||||
if (v[c] >= 0.f) {
|
||||
indexPos[c].push_back(i);
|
||||
break;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
size_t idxMin = index.size();
|
||||
int minComp = -1;
|
||||
for (int c = 0; c < 3; ++c) {
|
||||
size_t test = std::max(indexNeg[c].size(), indexPos[c].size());
|
||||
if (test < idxMin && test < index.size() * 3 / 4) {
|
||||
minComp = c;
|
||||
idxMin = test;
|
||||
}
|
||||
}
|
||||
|
||||
if (minComp == -1) {
|
||||
MakeLeaf(mesh, index, *n);
|
||||
return n;
|
||||
}
|
||||
|
||||
n->left = RecursiveMakeNode<Node>(mesh, indexNeg[minComp]);
|
||||
n->right = RecursiveMakeNode<Node>(mesh, indexPos[minComp]);
|
||||
|
||||
return n;
|
||||
}
|
||||
|
||||
template <typename Node>
|
||||
std::unique_ptr<Node> OBBTreeBuilder::buildCol(const ColMesh& mesh)
|
||||
{
|
||||
std::vector<int> root = MakeRootTriangleIndex(mesh);
|
||||
return RecursiveMakeNode<Node>(mesh, root);
|
||||
std::unique_ptr<Node> OBBTreeBuilder::buildCol(const ColMesh& mesh) {
|
||||
std::vector<int> root = MakeRootTriangleIndex(mesh);
|
||||
return RecursiveMakeNode<Node>(mesh, root);
|
||||
}
|
||||
|
||||
template std::unique_ptr<DNAMP1::DCLN::Collision::Node>
|
||||
OBBTreeBuilder::buildCol<DNAMP1::DCLN::Collision::Node>(const ColMesh& mesh);
|
||||
|
||||
}
|
||||
} // namespace DataSpec
|
||||
|
||||
Reference in New Issue
Block a user