Commit a0925be8 authored by Alexis Girault's avatar Alexis Girault
Browse files

ENH: Implement PlaneToSphereCD

parent a27197f4
......@@ -4,6 +4,7 @@
include(imstkAddLibrary)
imstk_add_library( Collision
DEPENDS
Geometry
SceneElements
)
......
......@@ -26,10 +26,17 @@
namespace imstk {
struct VertexNormalCollisionData
struct PositionDirectionCollisionData
{
Vec3d position;
Vec3d direction;
double penetrationDepth;
};
struct VertexDirectionCollisionData
{
size_t vertexId;
Vec3d normal;
Vec3d direction;
double penetrationDepth;
};
......@@ -49,9 +56,10 @@ struct EdgeEdgeCollisionData
struct CollisionData
{
std::vector<VertexNormalCollisionData> VNColData; //!< Vertex Normal collision data
std::vector<VertexTriangleCollisionData> VTColData; //!< Vertex Triangle collision data
std::vector<EdgeEdgeCollisionData> EEColData; //!< Edge Edge collision data
std::vector<PositionDirectionCollisionData> PDColData; //!< Position Direction collision data
std::vector<VertexDirectionCollisionData> VDColData; //!< Vertex Direction collision data
std::vector<VertexTriangleCollisionData> VTColData; //!< Vertex Triangle collision data
std::vector<EdgeEdgeCollisionData> EEColData; //!< Edge Edge collision data
};
}
......
......@@ -21,6 +21,11 @@
#include "imstkCollisionDetection.h"
#include "imstkPlaneToSphereCD.h"
#include "imstkCollidingObject.h"
#include "imstkGeometry.h"
#include <g3log/g3log.hpp>
namespace imstk {
......@@ -30,8 +35,31 @@ CollisionDetection::make_collision_detection(const Type& type,
std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection not implemented.";
return nullptr;
switch (type)
{
case Type::PlaneToSphere:
{
if (objA->getCollidingGeometry()->getType() != Geometry::Type::Plane ||
objB->getCollidingGeometry()->getType() != Geometry::Type::Sphere)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for PlaneToSphere collision detection.";
return nullptr;
}
return std::make_shared<PlaneToSphereCD>();
}break;
default:
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented.";
return nullptr;
}
}
}
const CollisionDetection::Type&
CollisionDetection::getType() const
{
return m_type;
}
}
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#include "imstkPlaneToSphereCD.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include "imstkPlane.h"
#include "imstkSphere.h"
#include <g3log/g3log.hpp>
namespace imstk {
void
PlaneToSphereCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB)
{
auto planeGeom = std::dynamic_pointer_cast<Plane>(objA->getCollidingGeometry());
auto sphereGeom = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
if (planeGeom == nullptr || sphereGeom == nullptr)
{
LOG(WARNING) << "PlaneToSphereCD::computeCollisionData error: invalid geometries.";
return;
}
// Get geometry properties
Vec3d sP = sphereGeom->getPosition();
double r = sphereGeom->getRadius();
Vec3d pP = planeGeom->getPosition();
Vec3d n = planeGeom->getNormal();
// Compute shortest distance
double d = (sP-pP).dot(n);
// Define sphere to plane direction
Vec3d dir = -n;
if( d < 0 )
{
d = -d;
dir = n;
}
// Return if no penetration
double penetrationDepth = r-d;
if ( penetrationDepth < 0)
{
return;
}
// Compute collision points
Vec3d pC = sP + dir*d;
Vec3d sC = sP + dir*r;
// Set collisionData
PositionDirectionCollisionData cdA = {pC, -dir, penetrationDepth};
PositionDirectionCollisionData cdB = {sC, dir, penetrationDepth};
colDataA.PDColData.push_back({pC, dir, penetrationDepth});
colDataB.PDColData.push_back(cdB);
LOG(DEBUG) << "Collision between "
<< objA->getName() << " & " << objB->getName()
<< " : " << penetrationDepth;
}
}
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version 2.0 (the "License");
you may not use this file except in compliance with the License.
You may obtain a copy of the License at
http://www.apache.org/licenses/LICENSE-2.0.txt
Unless required by applicable law or agreed to in writing, software
distributed under the License is distributed on an "AS IS" BASIS,
WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
See the License for the specific language governing permissions and
limitations under the License.
=========================================================================*/
#ifndef imstkPlaneToSphereCD_h
#define imstkPlaneToSphereCD_h
#include "imstkCollisionDetection.h"
#include <memory>
namespace imstk {
class CollidingObject;
class CollisionData;
class PlaneToSphereCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
PlaneToSphereCD() : CollisionDetection(CollisionDetection::Type::PlaneToSphere) {}
///
/// \brief Destructor
///
~PlaneToSphereCD() = default;
///
/// \brief Detect collision and compute collision data
///
void computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB) override;
};
}
#endif // ifndef imstkPlaneToSphereCD_h
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment