Commit d967380e authored by Ricardo Ortiz's avatar Ricardo Ortiz
Browse files

ENH: Debug AVMNidus application. Add imstk namespace.

parent 9acbd973
......@@ -26,6 +26,8 @@
#include "TimeIntegrators/OdeSystem.h"
#include "Solvers/SystemOfEquations.h"
namespace imstk {
Assembler::Assembler(std::shared_ptr <CollisionContext> collContext)
{
this->collisionContext = collContext;
......@@ -86,8 +88,8 @@ void Assembler::initSystem()
{
size_t dofSize = 0;
size_t nnz = 0;
std::vector<const core::SparseMatrixd *> systemMatrices;
std::vector<const core::Vectord *> rhsVector;
std::vector<const SparseMatrixd *> systemMatrices;
std::vector<const Vectord *> rhsVector;
for(auto & col : rows)
{
......@@ -123,16 +125,18 @@ void Assembler::initSystem()
}
//---------------------------------------------------------------------------
void Assembler::concatenateMatrix(const core::SparseMatrixd &Q,
core::SparseMatrixd &R,
void Assembler::concatenateMatrix(const SparseMatrixd &Q,
SparseMatrixd &R,
std::size_t i,
std::size_t j)
{
for(size_t k = i; k < i + Q.outerSize(); ++k)
{
for(core::SparseMatrixd::InnerIterator it(Q, k); it; ++it)
for(SparseMatrixd::InnerIterator it(Q, k); it; ++it)
{
R.insert(k, it.col() + j) = it.value();
}
}
}
}
......@@ -28,6 +28,8 @@
#include "Core/Matrix.h"
#include "Core/Vector.h"
namespace imstk {
// Forward declarations
class CollisionContext;
......@@ -42,7 +44,7 @@ class LinearSystem;
class Assembler : public CoreClass
{
public:
using SparseLinearSystem = LinearSystem<core::SparseMatrixd>;
using SparseLinearSystem = LinearSystem<SparseMatrixd>;
public:
///
......@@ -88,8 +90,8 @@ public:
/// \param i row offset
/// \param j column offset
///
void concatenateMatrix(const core::SparseMatrixd &Q,
core::SparseMatrixd &R,
void concatenateMatrix(const SparseMatrixd &Q,
SparseMatrixd &R,
size_t i,
size_t j);
......@@ -100,8 +102,10 @@ private:
///> Each system correspond to one type of interaction in the interaction graph.
std::vector<std::shared_ptr<SparseLinearSystem>> equationList;
std::vector<core::SparseMatrixd> A; ///> Matrices storage
std::vector<core::Vectord> b; ///> Right hand sides storage
std::vector<SparseMatrixd> A; ///> Matrices storage
std::vector<Vectord> b; ///> Right hand sides storage
};
}
#endif // ASSEMBLER_ASSEMBLER_H
......@@ -35,3 +35,7 @@ target_link_libraries(Assembler
TimeIntegrators
SceneModels
)
if(BUILD_TESTING)
add_subdirectory(UnitTests)
endif()
......@@ -17,5 +17,20 @@
// See the License for the specific language governing permissions and
// limitations under the License.
#include "Core/ConfigRendering.h"
#include <bandit/bandit.h>
#include "Core/MakeUnique.h"
#include "Assembler/Assembler.h"
using namespace bandit;
go_bandit([](){
describe("Collision pair", []() {
it("constructs ", []() {
std::unique_ptr<imstk::Assembler> assembler(imstk::make_unique<imstk::Assembler>());
AssertThat(assembler != nullptr, IsTrue());
});
});
});
......@@ -16,23 +16,15 @@
#
###########################################################################
imstk_add_library(External
SOURCES
FrameBufferObject/FrameBufferObject.cpp
FrameBufferObject/RenderBuffer.cpp
PUBLIC_HEADERS
FrameBufferObject/FrameBufferObject.h
FrameBufferObject/RenderBuffer.h
Moller/moller2.h
Moller/moller.h
Tree/tree.hh
)
set(Module Assembler)
target_compile_options(External
PRIVATE
$<$<CXX_COMPILER_ID:GNU>:-Wno-old-style-cast -Wno-multichar -Wno-type-limits>)
add_executable(${Module}UnitTestRunner
${BANDIT_RUNNER}
AssemblerSpec.cpp
)
target_link_libraries(External
PRIVATE
Core
)
target_compile_options(${Module}UnitTestRunner PRIVATE $<$<CXX_COMPILER_ID:GNU>:-Wno-old-style-cast -Wno-multichar -Wno-type-limits>)
target_link_libraries(${Module}UnitTestRunner Assembler)
simple_test(${Module} --reporter=spec)
......@@ -294,7 +294,6 @@ set(iMSTK_exports
Core
Devices
Event
External
Geometry
IO
Mesh
......@@ -302,7 +301,6 @@ set(iMSTK_exports
Simulators
Solvers
TimeIntegrators
VTKRendering
VirtualTools
SceneModels
SimulationManager
......@@ -320,7 +318,6 @@ add_subdirectory(Core)
add_subdirectory(Devices)
add_subdirectory(Event)
add_subdirectory(Examples)
add_subdirectory(External)
add_subdirectory(Geometry)
add_subdirectory(IO)
add_subdirectory(Mesh)
......@@ -330,7 +327,6 @@ add_subdirectory(SimulationManager)
add_subdirectory(Solvers)
add_subdirectory(SceneModels)
add_subdirectory(TimeIntegrators)
add_subdirectory(VTKRendering)
add_subdirectory(VirtualTools)
################################################################################
......
......@@ -24,22 +24,24 @@
#include "External/Moller/moller.h"
#include "External/Moller/moller2.h"
bool CollisionMoller::tri2tri( core::Vec3d &tri1Point1,
core::Vec3d &tri1Point2,
core::Vec3d &tri1Point3,
core::Vec3d &tri2Point1,
core::Vec3d &tri2Point2,
core::Vec3d &tri2Point3,
namespace imstk {
bool CollisionMoller::tri2tri( Vec3d &tri1Point1,
Vec3d &tri1Point2,
Vec3d &tri1Point3,
Vec3d &tri2Point1,
Vec3d &tri2Point2,
Vec3d &tri2Point3,
double &depth,
core::Vec3d &contactPoint,
core::Vec3d &normal)
Vec3d &contactPoint,
Vec3d &normal)
{
int coplanar;
core::Vec3d intersectionPoint2;
Vec3d intersectionPoint2;
short tri1SinglePointIndex;
short tri2SinglePointIndex;
core::Vec3d projPoint1;
core::Vec3d projPoint2;
Vec3d projPoint1;
Vec3d projPoint2;
if ( tri_tri_intersect_with_isectline_penetrationDepth( tri1Point1.data(),
tri1Point2.data(),
tri1Point3.data(),
......@@ -62,22 +64,22 @@ bool CollisionMoller::tri2tri( core::Vec3d &tri1Point1,
return false;
}
bool CollisionMoller::tri2tri( core::Vec3d &p_tri1Point1,
core::Vec3d &p_tri1Point2,
core::Vec3d &p_tri1Point3,
core::Vec3d &p_tri2Point1,
core::Vec3d &p_tri2Point2,
core::Vec3d &p_tri2Point3,
bool CollisionMoller::tri2tri( Vec3d &p_tri1Point1,
Vec3d &p_tri1Point2,
Vec3d &p_tri1Point3,
Vec3d &p_tri2Point1,
Vec3d &p_tri2Point2,
Vec3d &p_tri2Point3,
int &coplanar,
core::Vec3d &p_intersectionPoint1,
core::Vec3d &p_intersectionPoint2,
Vec3d &p_intersectionPoint1,
Vec3d &p_intersectionPoint2,
short &p_tri1SinglePointIndex,
short &p_tri2SinglePointIndex,
core::Vec3d &p_projPoint1,
core::Vec3d &p_projPoint2 )
Vec3d &p_projPoint1,
Vec3d &p_projPoint2 )
{
double depth;
core::Vec3d normal;
Vec3d normal;
if ( tri_tri_intersect_with_isectline_penetrationDepth( p_tri1Point1.data(),
p_tri1Point2.data(),
p_tri1Point3.data(),
......@@ -97,7 +99,7 @@ bool CollisionMoller::tri2tri( core::Vec3d &p_tri1Point1,
return false;
}
bool CollisionMoller::tri2tri( core::Vec3d &p_tri1Point1, core::Vec3d &p_tri1Point2, core::Vec3d &p_tri1Point3, core::Vec3d &p_tri2Point1, core::Vec3d &p_tri2Point2, core::Vec3d &p_tri2Point3 )
bool CollisionMoller::tri2tri( Vec3d &p_tri1Point1, Vec3d &p_tri1Point2, Vec3d &p_tri1Point3, Vec3d &p_tri2Point1, Vec3d &p_tri2Point2, Vec3d &p_tri2Point3 )
{
if(tri_tri_intersect(p_tri1Point1.data(), p_tri1Point2.data(),
p_tri1Point3.data(), p_tri2Point1.data(),
......@@ -139,8 +141,8 @@ bool CollisionMoller::checkOverlapAABBAABB( AABB &aabbA, AABB &aabbB, AABB &resu
bool CollisionMoller::checkOverlapAABBAABB( const AABB &aabbA, const AABB &aabbB )
{
const core::Vec3d &min = aabbA.getMin();
const core::Vec3d &max = aabbA.getMax();
const Vec3d &min = aabbA.getMin();
const Vec3d &max = aabbA.getMax();
if ( min[0] > aabbB.aabbMax[0] ||
max[0] < aabbB.aabbMin[0] ||
......@@ -157,7 +159,7 @@ bool CollisionMoller::checkOverlapAABBAABB( const AABB &aabbA, const AABB &aabbB
}
}
bool CollisionMoller::checkOverlapAABBAABB( AABB &aabbA, core::Vec3d &p_vertex )
bool CollisionMoller::checkOverlapAABBAABB( AABB &aabbA, Vec3d &p_vertex )
{
if ( aabbA.aabbMin[0] <= p_vertex[0] && aabbA.aabbMax[0] >= p_vertex[0] &&
......@@ -172,7 +174,7 @@ bool CollisionMoller::checkOverlapAABBAABB( AABB &aabbA, core::Vec3d &p_vertex )
}
}
bool CollisionMoller::checkAABBPoint( const AABB &p_aabb, const core::Vec3d &p_v )
bool CollisionMoller::checkAABBPoint( const AABB &p_aabb, const Vec3d &p_v )
{
if ( p_v[0] >= p_aabb.aabbMin[0] &&
p_v[1] >= p_aabb.aabbMin[1] &&
......@@ -189,19 +191,19 @@ bool CollisionMoller::checkAABBPoint( const AABB &p_aabb, const core::Vec3d &p_v
}
}
bool CollisionMoller::checkLineTri( core::Vec3d &p_linePoint1, core::Vec3d &p_linePoint2, core::Vec3d &p_tri1Point1, core::Vec3d &p_tri1Point2, core::Vec3d &p_tri1Point3, core::Vec3d &p_interSection )
bool CollisionMoller::checkLineTri( Vec3d &p_linePoint1, Vec3d &p_linePoint2, Vec3d &p_tri1Point1, Vec3d &p_tri1Point2, Vec3d &p_tri1Point3, Vec3d &p_interSection )
{
//First find that ray intersect the polygon or not
int i1, i2;
core::Vec3d U, V;
Vec3d U, V;
double plane[4], P[3], u[3], v[3], alpha, beta;
//Now find the Normal vector
U = p_tri1Point2 - p_tri1Point1;
V = p_tri1Point3 - p_tri1Point1;
core::Vec3d cProd;
Vec3d cProd;
cProd = U.cross( V );
cProd.normalize();
......@@ -212,7 +214,7 @@ bool CollisionMoller::checkLineTri( core::Vec3d &p_linePoint1, core::Vec3d &p_li
//Now we got the plane equation and we can test whether our ray intersect or not
double d1, d2, d, t;
core::Vec3d dir;
Vec3d dir;
d1 = p_linePoint1.dot( cProd ) + plane[3];
d2 = p_linePoint2.dot( cProd ) + plane[3];
......@@ -286,12 +288,12 @@ bool CollisionMoller::checkLineTri( core::Vec3d &p_linePoint1, core::Vec3d &p_li
return false;
}
bool CollisionMoller::checkAABBTriangle( AABB &p_aabb, core::Vec3d &v1, core::Vec3d &v2, core::Vec3d &v3 )
bool CollisionMoller::checkAABBTriangle( AABB &p_aabb, Vec3d &v1, Vec3d &v2, Vec3d &v3 )
{
core::Vec3d boxhalfsize;
core::Vec3d boxCenter;
core::Matrix33d triverts;
Vec3d boxhalfsize;
Vec3d boxCenter;
Matrix33d triverts;
triverts << v1[0],v1[1],v1[2],
v2[0],v2[1],v2[2],
v3[0],v3[1],v3[2];
......@@ -307,3 +309,5 @@ bool CollisionMoller::checkAABBTriangle( AABB &p_aabb, core::Vec3d &v1, core::Ve
}
return false;
}
}
......@@ -25,42 +25,44 @@
#include "Core/Geometry.h"
#include "Core/Vector.h"
/// \brief Collision utililites
namespace imstk {
/// \brief Collision utilities
class CollisionMoller
{
public:
/// \brief triangle and triangle collision retursn intersection and projection points
static bool tri2tri(core::Vec3d &p_tri1Point1,
core::Vec3d &p_tri1Point2,
core::Vec3d &p_tri1Point3,
core::Vec3d &p_tri2Point1,
core::Vec3d &p_tri2Point2,
core::Vec3d &p_tri2Point3,
static bool tri2tri(Vec3d &p_tri1Point1,
Vec3d &p_tri1Point2,
Vec3d &p_tri1Point3,
Vec3d &p_tri2Point1,
Vec3d &p_tri2Point2,
Vec3d &p_tri2Point3,
int &coplanar,
core::Vec3d &p_intersectionPoint1,
core::Vec3d &p_intersectionPoint2,
Vec3d &p_intersectionPoint1,
Vec3d &p_intersectionPoint2,
short &p_tri1SinglePointIndex,
short &p_tri2SinglePointIndex,
core::Vec3d &p_projPoint1,
core::Vec3d &p_projPoint2);
Vec3d &p_projPoint1,
Vec3d &p_projPoint2);
static bool tri2tri(core::Vec3d &p_tri1Point1,
core::Vec3d &p_tri1Point2,
core::Vec3d &p_tri1Point3,
core::Vec3d &p_tri2Point1,
core::Vec3d &p_tri2Point2,
core::Vec3d &p_tri2Point3,
static bool tri2tri(Vec3d &p_tri1Point1,
Vec3d &p_tri1Point2,
Vec3d &p_tri1Point3,
Vec3d &p_tri2Point1,
Vec3d &p_tri2Point2,
Vec3d &p_tri2Point3,
double &depth,
core::Vec3d &contactPoint,
core::Vec3d &normal);
Vec3d &contactPoint,
Vec3d &normal);
/// \brief checks if the two triangles intersect
static bool tri2tri(core::Vec3d &p_tri1Point1,
core::Vec3d &p_tri1Point2,
core::Vec3d &p_tri1Point3,
core::Vec3d &p_tri2Point1,
core::Vec3d &p_tri2Point2,
core::Vec3d &p_tri2Point3);
static bool tri2tri(Vec3d &p_tri1Point1,
Vec3d &p_tri1Point2,
Vec3d &p_tri1Point3,
Vec3d &p_tri2Point1,
Vec3d &p_tri2Point2,
Vec3d &p_tri2Point3);
/// \brief check if the two AABB overlap returns encapsulating AABB of two
static bool checkOverlapAABBAABB(AABB &aabbA, AABB &aabbB, AABB &result);
......@@ -69,20 +71,22 @@ public:
static bool checkOverlapAABBAABB(const AABB &aabbA, const AABB &aabbB);
/// \brief check if the point p_vertex is inside the AABB
static inline bool checkOverlapAABBAABB(AABB &aabbA, core::Vec3d &p_vertex);
static inline bool checkOverlapAABBAABB(AABB &aabbA, Vec3d &p_vertex);
/// \brief checks if the line intersects the tirangle. returns if it is true. the intersection is returned in p_interSection
static bool checkLineTri(core::Vec3d &p_linePoint1,
core::Vec3d &p_linePoint2,
core::Vec3d &p_tri1Point1,
core::Vec3d &p_tri1Point2,
core::Vec3d &p_tri1Point3,
core::Vec3d &p_interSection);
static bool checkLineTri(Vec3d &p_linePoint1,
Vec3d &p_linePoint2,
Vec3d &p_tri1Point1,
Vec3d &p_tri1Point2,
Vec3d &p_tri1Point3,
Vec3d &p_interSection);
/// \brief checks if the triangles points are within the AABB
static bool checkAABBTriangle(AABB &p_aabb, core::Vec3d &v1, core::Vec3d &v2, core::Vec3d &v3);
static bool checkAABBTriangle(AABB &p_aabb, Vec3d &v1, Vec3d &v2, Vec3d &v3);
static bool checkAABBPoint(const AABB &p_aabb, const core::Vec3d &p_v);
static bool checkAABBPoint(const AABB &p_aabb, const Vec3d &p_v);
};
}
#endif // COLLISION_COLLISIONMOLLER_H
......@@ -21,6 +21,8 @@
#include "Collision/SurfaceTree.h"
#include "Core/CollisionConfig.h"
namespace imstk {
//----------------------------------------------------------------------------------------
MeshCollisionModel::MeshCollisionModel()
{
......@@ -73,14 +75,14 @@ void MeshCollisionModel::initAABBTree(const int& numLevels)
}
//----------------------------------------------------------------------------------------
const core::Vec3d& MeshCollisionModel::getSurfaceNormal(size_t i) const
const Vec3d& MeshCollisionModel::getSurfaceNormal(size_t i) const
{
auto surfaceMesh = std::static_pointer_cast<SurfaceMesh>(this->mesh);
return surfaceMesh->getTriangleNormal(i);
}
//----------------------------------------------------------------------------------------
std::array<core::Vec3d,3>
std::array<Vec3d,3>
MeshCollisionModel::getElementPositions(size_t i) const
{
auto surfaceMesh = std::static_pointer_cast<SurfaceMesh>(this->mesh);
......@@ -133,3 +135,5 @@ std::shared_ptr< CollisionGroup >& MeshCollisionModel::getCollisionGroup()
{
return this->collisionGroup;
}
}
......@@ -28,6 +28,7 @@
#include "Collision/OctreeCell.h"
#include "Geometry/MeshModel.h"
namespace imstk {
class CollisionGroup;
template<typename T>
......@@ -80,12 +81,12 @@ public:
///
/// @brief Returns normal vectors for triangles on mesh surface
///
const core::Vec3d& getSurfaceNormal(size_t i) const;
const Vec3d& getSurfaceNormal(size_t i) const;
///
/// @brief Returns array of vertices for triangle on surface
///
std::array<core::Vec3d,3> getElementPositions(size_t i) const;
std::array<Vec3d,3> getElementPositions(size_t i) const;
///
/// \brief Set/get bounding box
......@@ -127,4 +128,6 @@ private:
std::shared_ptr<CollisionGroup> collisionGroup;
};
}
#endif // COLLISION_MESHMODEL_H
......@@ -28,6 +28,8 @@
// STL includes
#include <vector>
namespace imstk {
void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> pairs)
{
auto meshA = std::static_pointer_cast<MeshCollisionModel>(pairs->getFirst());
......@@ -37,8 +39,8 @@ void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> p
intersectionNodes = meshA->getAABBTree()->getIntersectingNodes(meshB->getAABBTree());
double depth;
core::Vec3d normal;
core::Vec3d contactPoint;
Vec3d normal;
Vec3d contactPoint;
for(auto & intersection : intersectionNodes)
{
auto nodeA = intersection.first;
......@@ -52,7 +54,7 @@ void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> p
for(const auto & i : triangleListA)
{
const core::Vec3d& normalA = meshA->getSurfaceNormal(i);
const Vec3d& normalA = meshA->getSurfaceNormal(i);
if(normalA.isZero())
{
continue;
......@@ -61,7 +63,7 @@ void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> p
auto verticesA = meshA->getElementPositions(i);
for(auto & j : triangleListB)
{
const core::Vec3d& normalB = meshB->getSurfaceNormal(j);
const Vec3d& normalB = meshB->getSurfaceNormal(j);
if(normalB.isZero())
{
continue;
......@@ -81,3 +83,5 @@ void MeshToMeshCollision::doComputeCollision(std::shared_ptr<CollisionManager> p
}
}
}
}
......@@ -23,6 +23,8 @@
// iMSTK includes
#include "Core/CollisionDetection.h"
namespace imstk {
class CollisionManager;
///
......@@ -42,4 +44,6 @@ private:
};
}
#endif // COLLISION_MESHTOMESHCOLLISION_H
......@@ -25,6 +25,8 @@
// iMSTK includes
#include "Collision/CollisionMoller.h"
namespace imstk {
OctreeCell::OctreeCell() : BaseType()
{
......@@ -35,17 +37,17 @@ OctreeCell::~OctreeCell()
}
core::Vec3d &OctreeCell::getCenter()
Vec3d &OctreeCell::getCenter()
{
return cube.center;
}
const core::Vec3d &OctreeCell::getCenter() const
const Vec3d &OctreeCell::getCenter() const
{
return cube.center;
}
void OctreeCell::setCenter( const core::Vec3d &center )
void OctreeCell::setCenter( const Vec3d &center )
{
cube.center = center;
}
......@@ -75,7 +77,7 @@ void OctreeCell::expand( const double expandScale )
cube.expand( expandScale );
}
bool OctreeCell::isCollidedWithTri(const core::Vec3d &v0, const core::Vec3d &v1, const core::Vec3d &v2 ) const
bool OctreeCell::isCollidedWithTri(const Vec3d &v0, const Vec3d &v1, const Vec3d &v2 ) const
{
Eigen::AlignedBox3d box;
box.min() = cube.leftMinCorner();
......@@ -121,3 +123,5 @@ void OctreeCell::setCube(const Cube &otherCube)
this->cube.center = otherCube.center;
this->cube.sideLength = otherCube.sideLength;
}
}
......@@ -28,6 +28,8 @@
#include "Collision/SurfaceTreeCell.h"
#include "Core/Geometry.h"
namespace imstk {
/// \brief cell of an octree
class OctreeCell : public SurfaceTreeCell<OctreeCell>
{
......@@ -51,13 +53,13 @@ public:
void setCube(const Cube &other);
/// \brief get the center of the octree cell
core::Vec3d &getCenter();
Vec3d &getCenter();