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

ENH: Implement SphereToSphereCD

parent 3a79f8c6
......@@ -22,6 +22,7 @@
#include "imstkCollisionDetection.h"
#include "imstkPlaneToSphereCD.h"
#include "imstkSphereToSphereCD.h"
#include "imstkCollidingObject.h"
#include "imstkGeometry.h"
......@@ -48,6 +49,17 @@ CollisionDetection::make_collision_detection(const Type& type,
}
return std::make_shared<PlaneToSphereCD>();
}break;
case Type::SphereToSphere:
{
if (objA->getCollidingGeometry()->getType() != Geometry::Type::Sphere ||
objB->getCollidingGeometry()->getType() != Geometry::Type::Sphere)
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: "
<< "invalid object geometries for SphereToSphere collision detection.";
return nullptr;
}
return std::make_shared<SphereToSphereCD>();
}break;
default:
{
LOG(WARNING) << "CollisionDetection::make_collision_detection error: type not implemented.";
......
......@@ -35,7 +35,8 @@ public:
enum class Type
{
PlaneToSphere
PlaneToSphere,
SphereToSphere
};
///
......
/*=========================================================================
Library: iMSTK
Copyright (c) Kitware, Inc. & Center for Modeling, Simulation,
& Imaging in Medicine, Rensselaer Polytechnic Institute.
Licensed under the Apache License, Version B.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-B.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 "imstkSphereToSphereCD.h"
#include "imstkCollidingObject.h"
#include "imstkCollisionData.h"
#include "imstkSphere.h"
#include <g3log/g3log.hpp>
namespace imstk {
void
SphereToSphereCD::computeCollisionData(std::shared_ptr<CollidingObject> objA,
std::shared_ptr<CollidingObject> objB,
CollisionData& colDataA,
CollisionData& colDataB)
{
auto sphereGeomA = std::dynamic_pointer_cast<Sphere>(objA->getCollidingGeometry());
auto sphereGeomB = std::dynamic_pointer_cast<Sphere>(objB->getCollidingGeometry());
// Geometries check
if (sphereGeomA == nullptr || sphereGeomB == nullptr)
{
LOG(WARNING) << "SphereToSphereCD::computeCollisionData error: invalid geometries.";
return;
}
// Get geometry properties
Vec3d sAP = sphereGeomA->getPosition();
double rA = sphereGeomA->getRadius() * sphereGeomA->getScaling();
Vec3d sBP = sphereGeomB->getPosition();
double rB = sphereGeomB->getRadius() * sphereGeomB->getScaling();
// Compute direction vector
Vec3d dirBToA = sAP - sBP;
// Compute shortest distance
double d = dirBToA.norm();
// Return if no penetration
double penetrationDepth = rA+rB-d;
if ( penetrationDepth <= 0)
{
return;
}
// Compute collision points
dirBToA.normalize();
Vec3d sAC = sAP - dirBToA*rA;
Vec3d sBC = sBP + dirBToA*rB;
// Set collisionData
colDataA.PDColData.push_back({sAC, dirBToA, penetrationDepth});
colDataB.PDColData.push_back({sBC, -dirBToA, 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 imstkSphereToSphereCD_h
#define imstkSphereToSphereCD_h
#include "imstkCollisionDetection.h"
#include <memory>
namespace imstk {
class CollidingObject;
class CollisionData;
class SphereToSphereCD : public CollisionDetection
{
public:
///
/// \brief Constructor
///
SphereToSphereCD() : CollisionDetection(CollisionDetection::Type::SphereToSphere) {}
///
/// \brief Destructor
///
~SphereToSphereCD() = 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 imstkSphereToSphereCD_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