Commits

Daniel K. O. committed 2d79b2a

more code for trimesh

Comments (0)

Files changed (6)

include/kode/AxisAlignedBox.hpp

 
 #include <stdexcept>
 
+#include <kode/Matrix3.hpp>
 #include <kode/Vector3.hpp>
 
+
 namespace kode {
 
     /**
 
     public:
 
-        struct Infinite {};
-
-
         /// Constructs an empty box.
         constexpr
         AxisAlignedBox() noexcept;
         AxisAlignedBox(const AxisAlignedBox& other) noexcept = default;
 
         /// Constructs an infinite box.
-        constexpr
-        AxisAlignedBox(Infinite) noexcept;
+        static constexpr
+        AxisAlignedBox Infinite() noexcept;
+
 
 
         constexpr
         constexpr
         Vector3 getCorner(unsigned i);
 
-/*
-  TODO
-        void transform(const Matrix3& m);
-        AxisAlignedBox transformed(const Matrix3& m) const;
-*/
+        inline
+        void transform(const Matrix3& r, const Vector3& p);
+
+        inline
+        AxisAlignedBox transformed(const Matrix3& r, const Vector3& p) const;
 
         /// Sets it as empty.
         inline
 
 
     constexpr
-    AxisAlignedBox::AxisAlignedBox(Infinite) noexcept :
-        extent{Extent::Infinite}
-    {}
+    AxisAlignedBox
+    AxisAlignedBox::Infinite() noexcept
+    {
+        return AxisAlignedBox(Extent::Infinite);
+    }
 
 
     constexpr
         }
     }
 
+
+    inline
+    void
+    AxisAlignedBox::transform(const Matrix3& r, const Vector3& p)
+    {
+        if (extent == Extent::Finite) {
+            const Vector3 center = getCenter();
+            const Vector3 hsize = getHalfSize();
+            const Vector3 newHSize = abs(r) * hsize;
+            setBounds(center + p - newHSize, center + p + newHSize);
+        }
+    }
+
+
+    inline
+    AxisAlignedBox
+    AxisAlignedBox::transformed(const Matrix3& r, const Vector3& p) const
+    {
+        if (extent == Extent::Finite) {
+            const Vector3 center = getCenter();
+            const Vector3 hsize = getHalfSize();
+            const Vector3 newHSize = abs(r) * hsize;
+            return AxisAlignedBox(center + p - newHSize, center + p + newHSize);
+        } else
+            return *this;
+    }
+
 }
 
 #endif

include/kode/collision/Trimesh.hpp

 #define KODE_BOX_H
 
 #include <memory>
+#include <cstdint>
 
 #include <kode/collision/Geom.hpp>
 
 namespace kode {
 
+    using TriIndex = std::uint16_t;
+
     class Trimesh : public Geom {
     public:
         struct Data {
             struct Impl;
 
             std::unique_ptr<Impl> pimpl;
+        public:
+            Data();
+            Data(const float* verts, unsigned nVerts, unsigned vertStride,
+                 const TriIndex* indices, unsigned triCount, unsigned triStride);
+            ~Data() = default;
 
-            Data();
             Data(Data&&) noexcept = default;
             Data& operator=(Data&&) noexcept = default;
-            ~Data() = default;
+
+
+            void build(const float* verts, unsigned nVerts, unsigned vertStride,
+                       const TriIndex* indices, unsigned indexCount, unsigned triStride);
+
+
+            void preprocess();
+
+            friend class Trimesh;
         };
 
 

include/kode/solvers/QuickStepSolver.hpp

         Real relaxation = 0.75;
         unsigned maxIterations = 20;
 
+        struct Impl;
+
+        std::unique_ptr<Impl> pimpl;
+
     public:
 
-        struct Impl {
-            virtual ~Impl() {}
-            virtual void solve(const World& world,
-                               std::vector<Body*>& bodies,
-                               std::vector<Joint*>& joints,
-                               Real relaation,
-                               unsigned maxIterations) = 0;
-
-        };
-    private:
-        std::unique_ptr<Impl> impl;
-    public:
-
 
         QuickStepSolver();
+        ~QuickStepSolver(); // must not be inlined because Impl is incomplete here
 
         std::string getName() const noexcept override;
 

src/collision/Trimesh.cpp

 */
 
 #include <utility>
-#include <cstdint>
+#include <vector>
 
 
 #include <kode/collision/Trimesh.hpp>
 
 namespace kode {
 
-    class Trimesh::Data::Impl {
+    namespace {
+
+        enum : std::uint8_t {
+            kEdge0 = 0x1,
+            kEdge1 = 0x2,
+            kEdge2 = 0x4,
+            kVert0 = 0x8,
+            kVert1 = 0x10,
+            kVert2 = 0x20,
+            kUseAll = 0xFF
+        };
+
+
+        struct EdgeRecord {
+            unsigned vertIdx1; // Index into vertex array for this edges vertices
+            unsigned vertIdx2;
+            TriIndex triIdx; // Index into triangle array for triangle this edge belongs to
+
+            std::uint8_t edgeFlags;	
+            std::uint8_t vert1Flags;
+            std::uint8_t vert2Flags;
+            bool concave;
+        };
+
+
+
+        void setupEdge(EdgeRecord* edge, int edgeIdx, int triIdx, const TriIndex* vertIdxs)
+        {
+            if (edgeIdx == 0) {
+                edge->edgeFlags  = kEdge0;
+                edge->vert1Flags = kVert0;
+                edge->vert2Flags = kVert1;
+                edge->vertIdx1 = vertIdxs[0];
+                edge->vertIdx2 = vertIdxs[1];
+            } else if (edgeIdx == 1) {
+                edge->edgeFlags  = kEdge1;
+                edge->vert1Flags = kVert1;
+                edge->vert2Flags = kVert2;
+                edge->vertIdx1 = vertIdxs[1];
+                edge->vertIdx2 = vertIdxs[2];
+            } else if (edgeIdx == 2) {
+                edge->edgeFlags  = kEdge2;
+                edge->vert1Flags = kVert2;
+                edge->vert2Flags = kVert0;
+                edge->vertIdx1 = vertIdxs[2];
+                edge->vertIdx2 = vertIdxs[0];
+            }
+
+            // Make sure vert index 1 is less than index 2 (for easier sorting)
+            if (edge->vertIdx1 > edge->vertIdx2)
+            {
+                unsigned int tempIdx = edge->vertIdx1;
+                edge->vertIdx1 = edge->vertIdx2;
+                edge->vertIdx2 = tempIdx;
+
+                std::uint8_t tempFlags = edge->vert1Flags;
+                edge->vert1Flags = edge->vert2Flags;
+                edge->vert2Flags = tempFlags;
+            }
+
+            edge->triIdx = triIdx;
+            edge->concave = false;
+        }
+
+        // Get the vertex opposite this edge in the triangle
+        Point
+        getOppositeVert(const EdgeRecord* edge, const Point* vertices[])
+        {
+            if ((edge->vert1Flags == kVert0 && edge->vert2Flags == kVert1) ||
+                (edge->vert1Flags == kVert1 && edge->vert2Flags == kVert0)) {
+                return *vertices[2];
+            } else if ((edge->vert1Flags == kVert1 && edge->vert2Flags == kVert2) ||
+                       (edge->vert1Flags == kVert2 && edge->vert2Flags == kVert1)) {
+                return *vertices[0];
+            } else
+                return *vertices[1];
+        }
+
+    }
+
+
+
+    struct Trimesh::Data::Impl {
         opc::Model BVTree;
         opc::MeshInterface Mesh;
 
         /* aabb in model space */
-        Vector3 AABBCenter;
-        Vector3 AABBExtents;
+        AxisAlignedBox aabb;
 
         // data for use in collision resolution
         const void* normals;
-        std::unique_ptr<uint8[]> useFlags;
+        std::vector<uint8> useFlags;
 
-    public:
 
-        void build(const void* vertices, int vertexStride, int vertexCount, 
-                   const void* indices, int indexCount,
-                   int triStride, const void* normals,  bool single);
+        void build(const float* vertices, unsigned vertexCount, unsigned vertexStride,
+                   const kode::TriIndex* indices, unsigned triCount, unsigned triStride);
+
+        void preprocess();
+
 
         unsigned triangleCount()
         {
 
 
 
-    
-
-
     void
     Trimesh::fetchTransformedTriangle(int index, Vector3 out[3])
     {
 
 
 
-
-    Trimesh::Data::Data() :
-        pimpl{new Impl}
-    {}
-
-
     void
-    Trimesh::Data::Impl::build(const void* vertices, int vertexStride, int vertexCount,
-                               const void* indices, int indexCount, int triStride,
-                               const void* inNormals,
-                               bool single)
+    Trimesh::Data::Impl::build(const float* vertices, unsigned vertexCount, unsigned vertexStride,
+                               const kode::TriIndex* indices, unsigned triCount, unsigned triStride)
     {
-        Mesh.SetNbTriangles(indexCount / 3);
+        Mesh.SetNbTriangles(triCount);
         Mesh.SetNbVertices(vertexCount);
-        Mesh.SetPointers((IndexedTriangle*)indices, (Point*)vertices);
+        Mesh.SetPointers(reinterpret_cast<const IndexedTriangle*>(indices),
+                         reinterpret_cast<const Point*>(vertices));
         Mesh.SetStrides(triStride, vertexStride);
-        Mesh.SetSingle(single);
+        Mesh.SetSingle(true);
 
         // Build tree
         opc::BuildSettings Settings;
         BVTree.Build(TreeBuilder);
 
         // compute model space AABB
-        Vector3 AABBMax, AABBMin;
-        AABBMax[0] = AABBMax[1] = AABBMax[2] = -Math::Infinity;
-        AABBMin[0] = AABBMin[1] = AABBMin[2] =  Math::Infinity;
-        if (single) {
-            const char* verts = (const char*)vertices;
-            for(int i = 0; i < vertexCount; ++i) {
-                const float* v = (const float*)verts;
-                if( v[0] > AABBMax[0] ) AABBMax[0] = v[0];
-                if( v[1] > AABBMax[1] ) AABBMax[1] = v[1];
-                if( v[2] > AABBMax[2] ) AABBMax[2] = v[2];
-                if( v[0] < AABBMin[0] ) AABBMin[0] = v[0];
-                if( v[1] < AABBMin[1] ) AABBMin[1] = v[1];
-                if( v[2] < AABBMin[2] ) AABBMin[2] = v[2];
-                verts += vertexStride;
-            }
-        } else {
-            const char* verts = (const char*)vertices;
-            for (int i = 0; i < vertexCount; ++i) {
-                const double* v = (const double*)verts;
-                if( v[0] > AABBMax[0] ) AABBMax[0] = v[0];
-                if( v[1] > AABBMax[1] ) AABBMax[1] = v[1];
-                if( v[2] > AABBMax[2] ) AABBMax[2] = v[2];
-                if( v[0] < AABBMin[0] ) AABBMin[0] = v[0];
-                if( v[1] < AABBMin[1] ) AABBMin[1] = v[1];
-                if( v[2] < AABBMin[2] ) AABBMin[2] = v[2];
-                verts += vertexStride;
+        aabb.clear();
+        const char* vert = reinterpret_cast<const char*>(vertices);
+        for(unsigned i = 0; i < vertexCount; ++i) {
+            const float* v = reinterpret_cast<const float*>(vert);
+            aabb.merge(v[0], v[1], v[2]);
+            vert += vertexStride;
+        }
+    }
+
+
+    void
+    Trimesh::Data::Impl::preprocess()
+    {
+        // If this mesh has already been preprocessed, exit
+        if (!useFlags.empty())
+            return;
+
+        unsigned numTris = triangleCount();
+        unsigned numEdges = numTris * 3;
+
+        useFlags.resize(numTris);
+
+        std::vector<EdgeRecord> records(numEdges);
+
+        // Make a list of every edge in the mesh
+        const char* triBase = reinterpret_cast<const char*>(Mesh.GetTris());
+        const unsigned tristride = Mesh.GetTriStride();
+        for (unsigned i = 0; i < numTris; i++) {
+            const IndexedTriangle* tris = reinterpret_cast<const IndexedTriangle*>(triBase);
+
+            setupEdge(&records[i*3],   0, i, tris->mVRef);
+            setupEdge(&records[i*3+1], 1, i, tris->mVRef);
+            setupEdge(&records[i*3+2], 2, i, tris->mVRef);
+
+            triBase += tristride;
+        }
+
+        // Sort the edges, so the ones sharing the same verts are beside each other
+        std::sort(records.begin(), records.end(), [](const EdgeRecord& a, const EdgeRecord& b)
+                  {
+                      return a.vertIdx1 < b.vertIdx1 || 
+                                          (a.vertIdx1 == b.vertIdx1 && a.vertIdx2 < b.vertIdx2);
+                  });
+
+        // Go through the sorted list of edges and flag all the edges and vertices that we need to use
+        for (unsigned int i = 0; i < numEdges; i++) {
+            EdgeRecord* rec1 = &records[i];
+            EdgeRecord* rec2 = nullptr;
+            if (i < numEdges - 1)
+                rec2 = &records[i+1];
+
+            if (rec2 &&
+                rec1->vertIdx1 == rec2->vertIdx1 &&
+                rec1->vertIdx2 == rec2->vertIdx2) {
+
+                opc::VertexPointers vp;
+                opc::ConversionArea vc;
+                Mesh.GetTriangle(vp, rec1->triIdx, vc);
+
+                // Get the normal of the first triangle
+                Point triNorm = (*vp.Vertex[2] - *vp.Vertex[1]) ^ (*vp.Vertex[0] - *vp.Vertex[1]);
+                triNorm.Normalize();
+
+                // Get the vert opposite this edge in the first triangle
+                Point oppositeVert1 = getOppositeVert(rec1, vp.Vertex);
+
+                // Get the vert opposite this edge in the second triangle
+                Mesh.GetTriangle(vp, rec2->triIdx, vc);
+                Point oppositeVert2 = getOppositeVert(rec2, vp.Vertex);
+
+                float dot = triNorm.Dot((oppositeVert2 - oppositeVert1).Normalize());
+
+                // We let the dot threshold for concavity get slightly negative to allow for rounding errors
+                const float kConcaveThresh = -0.000001f;
+
+                // This is a concave edge, leave it for the next pass
+                if (dot >= kConcaveThresh)
+                    rec1->concave = true;
+                // If this is a convex edge, mark its vertices and edge as used
+                else
+                    useFlags[rec1->triIdx] |= rec1->vert1Flags | rec1->vert2Flags | rec1->edgeFlags;
+
+                // Skip the second edge
+                i++;
+            } else {
+                // This is a boundary edge
+                useFlags[rec1->triIdx] |= rec1->vert1Flags | rec1->vert2Flags | rec1->edgeFlags;
             }
         }
-        AABBCenter[0] = (AABBMin[0] + AABBMax[0]) / 2;
-        AABBCenter[1] = (AABBMin[1] + AABBMax[1]) / 2;
-        AABBCenter[2] = (AABBMin[2] + AABBMax[2]) / 2;
-        AABBExtents[0] = AABBMax[0] - AABBCenter[0];
-        AABBExtents[1] = AABBMax[1] - AABBCenter[1];
-        AABBExtents[2] = AABBMax[2] - AABBCenter[2];
 
-        // user data (not used by OPCODE)
-        normals = (Real *) inNormals;
+        // Go through the list once more, and take any edge we marked as concave and
+        // clear it's vertices flags in any triangles they're used in
+        for (unsigned int i = 0; i < numEdges; i++) {
+            EdgeRecord& er = records[i];
 
+            if (er.concave) {
+                for (unsigned int j = 0; j < numEdges; j++) {
+                    EdgeRecord& curER = records[j];
+
+                    if (curER.vertIdx1 == er.vertIdx1 ||
+                        curER.vertIdx1 == er.vertIdx2)
+                        useFlags[curER.triIdx] &= ~curER.vert1Flags;
+
+                    if (curER.vertIdx2 == er.vertIdx1 ||
+                        curER.vertIdx2 == er.vertIdx2)
+                        useFlags[curER.triIdx] &= ~curER.vert2Flags;
+                }
+            }
+        }
     }
 
 
+    Trimesh::Data::Data() :
+        pimpl{new Impl}
+    {}
 
 
 
+    Trimesh::Data::Data(const float* verts, unsigned nVerts, unsigned vertStride,
+                        const TriIndex* indices, unsigned triCount, unsigned triStride) :
+        Data()
+    {
+        build(verts, nVerts, vertStride, indices, triCount, triStride);
+    }
+
+
+    void
+    Trimesh::Data::build(const float* verts, unsigned nVerts, unsigned vertStride,
+                         const TriIndex* indices, unsigned triCount, unsigned triStride)
+    {
+        pimpl->build(verts, nVerts, vertStride, indices, triCount, triStride);
+    }
+
+
+
+    void
+    Trimesh::Data::preprocess()
+    {
+        pimpl->preprocess();
+    }
+
 
     Trimesh::Trimesh(Data&& newData)
     {
     void
     Trimesh::computeAABB() noexcept
     {
-        assert(!"Not yet implemented!");
+        if (data)
+            aabb = data->pimpl->aabb.transformed(getLocalAxes(), getPosition());
+        else
+            aabb.clear();
     }
 
 

src/solvers/PQuickStepSolver.cpp

     {
         std::launch policy = joints.size() > 2 ? std::launch::async : std::launch::deferred;
         auto f = std::async(policy,
-                            &PQuickStepSolver::threadSolve, this, std::cref(world), std::move(bodies), std::move(joints));
+                            &PQuickStepSolver::threadSolve,
+                            this,
+                            std::cref(world),
+                            std::move(bodies),
+                            std::move(joints));
         tasks.push_back(std::move(f));
     }
 

src/solvers/QuickStepSolver.cpp

 
 namespace kode {
 
-    namespace {
+    class QuickStepSolver::Impl : public quickstep::Core {
 
+    public:
+        void solve(const World& world,
+                   std::vector<Body*>& bodies,
+                   std::vector<Joint*>& joints,
+                   Real relaxation,
+                   unsigned maxIterations)
+        {
+            quickstep::Core::solve(world,
+                                   bodies,
+                                   joints,
+                                   relaxation,
+                                   maxIterations);
+        }
 
-        class QSImpl : public QuickStepSolver::Impl,
-                       quickstep::Core {
+    };
 
-        public:
-            void solve(const World& world,
-                       std::vector<Body*>& bodies,
-                       std::vector<Joint*>& joints,
-                       Real relaxation,
-                       unsigned maxIterations)
-            {
-                quickstep::Core::solve(world,
-                                       bodies,
-                                       joints,
-                                       relaxation,
-                                       maxIterations);
-            }
 
-        };
-
-    }
 
 
 
     QuickStepSolver::QuickStepSolver() :
-        impl(new QSImpl)
+        pimpl(new Impl)
+    {}
+
+
+    QuickStepSolver::~QuickStepSolver()
     {}
 
 
                            std::vector<Body*>& bodies,
                            std::vector<Joint*>& joints)
     {
-        impl->solve(world, bodies, joints, getRelaxation(), getMaxIterations());
+        pimpl->solve(world, bodies, joints, getRelaxation(), getMaxIterations());
     }