Skip to content
GitLab
Projects
Groups
Snippets
/
Help
Help
Support
Community forum
Keyboard shortcuts
?
Submit feedback
Contribute to GitLab
Sign in / Register
Toggle navigation
Menu
Open sidebar
iMSTK
iMSTK
Commits
42a321fb
Commit
42a321fb
authored
Jul 28, 2021
by
Ye Han
🐼
Committed by
Andrew Wilson
Aug 03, 2021
Browse files
ENH: Add SurfaceMesh to Capsule CD
parent
91d09f63
Changes
6
Hide whitespace changes
Inline
Side-by-side
Examples/PBD/PBDClothCollision/pbdClothCollisionExample.cpp
View file @
42a321fb
...
...
@@ -42,6 +42,7 @@
#include
"imstkSimulationManager.h"
#include
"imstkSphere.h"
#include
"imstkSurfaceMesh.h"
#include
"imstkSurfaceMeshToCapsuleCD.h"
#include
"imstkSurfaceMeshToSphereCD.h"
#include
"imstkVisualModel.h"
#include
"imstkVTKViewer.h"
...
...
@@ -346,6 +347,25 @@ main()
scene
->
buildTaskGraph
();
scene
->
reset
();
}
// Switch to sphere vs surface and reset
else
if
(
e
->
m_key
==
'6'
)
{
for
(
int
i
=
0
;
i
<
4
;
i
++
)
{
collisionObj
->
getVisualModel
(
i
)
->
hide
();
}
collisionObj
->
getVisualModel
(
0
)
->
show
();
collisionObj
->
setCollidingGeometry
(
capsule
);
auto
capsuleCD
=
std
::
make_shared
<
SurfaceMeshToCapsuleCD
>
();
capsuleCD
->
setInputGeometryA
(
clothObj
->
getCollidingGeometry
());
capsuleCD
->
setInputGeometryB
(
capsule
);
pbdInteraction
->
setCollisionDetection
(
capsuleCD
);
pbdInteraction
->
getCollisionHandlingA
()
->
setInputCollisionData
(
capsuleCD
->
getCollisionData
());
scene
->
buildTaskGraph
();
scene
->
reset
();
}
});
driver
->
start
();
...
...
Source/CollisionDetection/CollisionDetection/imstkCollisionUtils.cpp
View file @
42a321fb
...
...
@@ -171,6 +171,37 @@ pointTriangleClosestDistance(const Vec3d& point, const Vec3d& x1, const Vec3d& x
}
}
Vec3d
closestPointOnSegment
(
const
Vec3d
&
point
,
const
Vec3d
&
x1
,
const
Vec3d
&
x2
,
int
&
caseType
)
{
Vec3d
dx
=
x2
-
x1
;
double
m2
=
dx
.
squaredNorm
();
if
(
m2
<
Real
(
1e-20
))
{
caseType
=
0
;
return
x1
;
}
// find parameter value of closest point on segment
double
s12
=
dx
.
dot
(
x2
-
point
)
/
m2
;
if
(
s12
<
0
)
{
s12
=
0
;
caseType
=
1
;
}
else
if
(
s12
>
1.0
)
{
s12
=
1.0
;
caseType
=
0
;
}
else
{
caseType
=
2
;
}
return
(
s12
*
x1
+
(
1.0
-
s12
)
*
x2
).
eval
();
}
Vec3d
closestPointOnTriangle
(
const
Vec3d
&
p
,
const
Vec3d
&
a
,
const
Vec3d
&
b
,
const
Vec3d
&
c
,
int
&
caseType
)
{
...
...
Source/CollisionDetection/CollisionDetection/imstkCollisionUtils.h
View file @
42a321fb
...
...
@@ -680,6 +680,14 @@ testSegmentTriangle(
return
(
t
>
EPSILON
&&
t
+
EPSILON
<
lAB
);
}
///
/// \brief Returns the closest position to point on segment a-b and the case
/// type=0: x1 is the closest point
/// type=1: x2 is the closest point
/// type=2: closest point is on x1-x2
///
Vec3d
closestPointOnSegment
(
const
Vec3d
&
point
,
const
Vec3d
&
x1
,
const
Vec3d
&
x2
,
int
&
caseType
);
///
/// \brief Returns the position closest to triangle a-b-c and the case
/// type=0: a is the closest point
...
...
Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToCapsuleCD.cpp
0 → 100644
View file @
42a321fb
/*=========================================================================
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
"imstkSurfaceMeshToCapsuleCD.h"
#include
"imstkCapsule.h"
#include
"imstkCollisionData.h"
#include
"imstkCollisionUtils.h"
#include
"imstkSurfaceMesh.h"
#include
<stdio.h>
namespace
imstk
{
SurfaceMeshToCapsuleCD
::
SurfaceMeshToCapsuleCD
()
{
setRequiredInputType
<
SurfaceMesh
>
(
0
);
setRequiredInputType
<
Capsule
>
(
1
);
}
void
SurfaceMeshToCapsuleCD
::
computeCollisionDataAB
(
std
::
shared_ptr
<
Geometry
>
geomA
,
std
::
shared_ptr
<
Geometry
>
geomB
,
CDElementVector
<
CollisionElement
>&
elementsA
,
CDElementVector
<
CollisionElement
>&
elementsB
)
{
std
::
shared_ptr
<
SurfaceMesh
>
surfMesh
=
std
::
dynamic_pointer_cast
<
SurfaceMesh
>
(
geomA
);
std
::
shared_ptr
<
Capsule
>
capsule
=
std
::
dynamic_pointer_cast
<
Capsule
>
(
geomB
);
const
Vec3d
&
capsulePos
=
capsule
->
getPosition
();
const
double
capsuleRadius
=
capsule
->
getRadius
();
const
double
capsuleLength
=
capsule
->
getLength
();
const
Quatd
&
capsuleOrientation
=
capsule
->
getOrientation
();
const
Vec3d
&
capsulePosA
=
capsulePos
-
0.5
*
capsuleLength
*
capsuleOrientation
.
toRotationMatrix
().
transpose
().
col
(
1
);
const
Vec3d
&
capsulePosB
=
capsulePos
+
(
capsulePos
-
capsulePosA
);
std
::
shared_ptr
<
VecDataArray
<
int
,
3
>>
indicesPtr
=
surfMesh
->
getTriangleIndices
();
const
VecDataArray
<
int
,
3
>&
indices
=
*
indicesPtr
;
std
::
shared_ptr
<
VecDataArray
<
double
,
3
>>
verticesPtr
=
surfMesh
->
getVertexPositions
();
const
VecDataArray
<
double
,
3
>&
vertices
=
*
verticesPtr
;
// \todo: Doesn't remove duplicate contacts (shared edges), refer to SurfaceMeshCD for easy method to do so
ParallelUtils
::
parallelFor
(
indices
.
size
(),
[
&
](
const
int
i
)
{
const
Vec3i
&
cell
=
indices
[
i
];
const
Vec3d
&
x1
=
vertices
[
cell
[
0
]];
const
Vec3d
&
x2
=
vertices
[
cell
[
1
]];
const
Vec3d
&
x3
=
vertices
[
cell
[
2
]];
// Choose the closest point on capsule axis to create a virtual sphere for CD
int
unusedCaseType
=
0
;
const
Vec3d
trianglePointA
=
CollisionUtils
::
closestPointOnTriangle
(
capsulePosA
,
x1
,
x2
,
x3
,
unusedCaseType
);
const
Vec3d
trianglePointB
=
CollisionUtils
::
closestPointOnTriangle
(
capsulePosB
,
x1
,
x2
,
x3
,
unusedCaseType
);
const
auto
segmentPointA
=
CollisionUtils
::
closestPointOnSegment
(
trianglePointA
,
capsulePosA
,
capsulePosB
,
unusedCaseType
);
const
auto
segmentPointB
=
CollisionUtils
::
closestPointOnSegment
(
trianglePointB
,
capsulePosA
,
capsulePosB
,
unusedCaseType
);
const
auto
distanceA
=
(
segmentPointA
-
trianglePointA
).
norm
();
const
auto
distanceB
=
(
segmentPointB
-
trianglePointB
).
norm
();
const
double
sphereRadius
=
capsuleRadius
;
Vec3d
spherePos
(
0
,
0
,
0
);
if
(
distanceA
<
distanceB
)
{
spherePos
=
segmentPointA
;
}
else
if
(
distanceA
>
distanceB
)
{
spherePos
=
segmentPointB
;
}
else
// parallel
{
spherePos
=
(
segmentPointA
+
segmentPointB
)
/
2
;
}
// This approach does a built in sphere sweep
// \todo: Spatial accelerators need to be abstracted
const
Vec3d
centroid
=
(
x1
+
x2
+
x3
)
/
3.0
;
// Find the maximal point from centroid for radius
const
double
rSqr1
=
(
centroid
-
x1
).
squaredNorm
();
const
double
rSqr2
=
(
centroid
-
x2
).
squaredNorm
();
const
double
rSqr3
=
(
centroid
-
x3
).
squaredNorm
();
const
double
triangleBoundingRadius
=
std
::
sqrt
(
std
::
max
(
rSqr1
,
std
::
max
(
rSqr2
,
rSqr3
)));
const
double
distSqr
=
(
centroid
-
spherePos
).
squaredNorm
();
const
double
rSum
=
triangleBoundingRadius
+
sphereRadius
;
if
(
distSqr
<
rSum
*
rSum
)
{
Vec3d
triangleContactPt
;
Vec2i
edgeContact
;
int
pointContact
;
int
caseType
=
CollisionUtils
::
testSphereToTriangle
(
spherePos
,
sphereRadius
,
cell
,
x1
,
x2
,
x3
,
triangleContactPt
,
edgeContact
,
pointContact
);
if
(
caseType
==
1
)
// Edge vs point on sphere
{
// Edge contact
CellIndexElement
elemA
;
elemA
.
ids
[
0
]
=
edgeContact
[
0
];
elemA
.
ids
[
1
]
=
edgeContact
[
1
];
elemA
.
idCount
=
2
;
elemA
.
cellType
=
IMSTK_EDGE
;
Vec3d
contactNormal
=
(
spherePos
-
triangleContactPt
);
const
double
dist
=
contactNormal
.
norm
();
const
double
penetrationDepth
=
sphereRadius
-
dist
;
contactNormal
/=
dist
;
PointDirectionElement
elemB
;
elemB
.
dir
=
contactNormal
;
// Direction to resolve sphere
elemB
.
pt
=
spherePos
-
sphereRadius
*
contactNormal
;
// Contact point on sphere
elemB
.
penetrationDepth
=
penetrationDepth
;
elementsA
.
safeAppend
(
elemA
);
elementsB
.
safeAppend
(
elemB
);
}
else
if
(
caseType
==
2
)
// Triangle vs point on sphere
{
// Face contact
CellIndexElement
elemA
;
elemA
.
ids
[
0
]
=
cell
[
0
];
elemA
.
ids
[
1
]
=
cell
[
1
];
elemA
.
ids
[
2
]
=
cell
[
2
];
elemA
.
idCount
=
3
;
elemA
.
cellType
=
IMSTK_TRIANGLE
;
Vec3d
contactNormal
=
(
spherePos
-
triangleContactPt
);
const
double
dist
=
contactNormal
.
norm
();
const
double
penetrationDepth
=
sphereRadius
-
dist
;
contactNormal
/=
dist
;
PointDirectionElement
elemB
;
elemB
.
dir
=
contactNormal
;
// Direction to resolve sphere
elemB
.
pt
=
spherePos
-
sphereRadius
*
contactNormal
;
// Contact point on sphere
elemB
.
penetrationDepth
=
penetrationDepth
;
elementsA
.
safeAppend
(
elemA
);
elementsB
.
safeAppend
(
elemB
);
}
else
if
(
caseType
==
3
)
{
Vec3d
contactNormal
=
(
spherePos
-
triangleContactPt
);
const
double
dist
=
contactNormal
.
norm
();
const
double
penetrationDepth
=
sphereRadius
-
dist
;
contactNormal
/=
dist
;
// Point contact
PointIndexDirectionElement
elemA
;
elemA
.
ptIndex
=
pointContact
;
elemA
.
dir
=
-
contactNormal
;
// Direction to resolve point
elemA
.
penetrationDepth
=
penetrationDepth
;
PointDirectionElement
elemB
;
elemB
.
pt
=
triangleContactPt
;
// Point on sphere
elemB
.
dir
=
contactNormal
;
// Direction to resolve point
elemB
.
penetrationDepth
=
penetrationDepth
;
elementsA
.
safeAppend
(
elemA
);
elementsB
.
safeAppend
(
elemB
);
}
}
},
vertices
.
size
()
>
1000
);
}
}
\ No newline at end of file
Source/CollisionDetection/CollisionDetection/imstkSurfaceMeshToCapsuleCD.h
0 → 100644
View file @
42a321fb
/*=========================================================================
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.
=========================================================================*/
#pragma once
#include
"imstkCollisionDetectionAlgorithm.h"
namespace
imstk
{
///
/// \class SurfaceMeshToCapsuleCD
///
/// \brief SurfaceMesh to Capsule collision detection
/// Generates vertex-triangle, point-edge, and point-point CD data
/// By default only generates contact data for the pointset.
///
class
SurfaceMeshToCapsuleCD
:
public
CollisionDetectionAlgorithm
{
public:
SurfaceMeshToCapsuleCD
();
virtual
~
SurfaceMeshToCapsuleCD
()
override
=
default
;
///
/// \brief Returns collision detection type string name
///
virtual
const
std
::
string
getTypeName
()
const
override
{
return
"SurfaceMeshToCapsuleCD"
;
}
protected:
///
/// \brief Compute collision data for AB simulatenously
///
virtual
void
computeCollisionDataAB
(
std
::
shared_ptr
<
Geometry
>
geomA
,
std
::
shared_ptr
<
Geometry
>
geomB
,
CDElementVector
<
CollisionElement
>&
elementsA
,
CDElementVector
<
CollisionElement
>&
elementsB
)
override
;
};
}
Source/CollisionDetection/imstkCDObjectFactory.cpp
View file @
42a321fb
...
...
@@ -37,6 +37,7 @@ limitations under the License.
#include
"imstkSphereToCylinderCD.h"
#include
"imstkSphereToSphereCD.h"
#include
"imstkSurfaceMesh.h"
#include
"imstkSurfaceMeshToCapsuleCD.h"
#include
"imstkSurfaceMeshToSphereCD.h"
#include
"imstkSurfaceMeshToSurfaceMeshCCD.h"
#include
"imstkSurfaceMeshToSurfaceMeshCD.h"
...
...
@@ -74,6 +75,7 @@ std::unordered_map<std::string, std::function<std::shared_ptr<CollisionDetection
IMSTK_MAKE_CD_CASE
(
SphereToSphereCD
,
Sphere
,
Sphere
),
//IMSTK_MAKE_CD_CASE(SurfaceMeshToSurfaceMeshCCD, SurfaceMesh, SurfaceMesh),
IMSTK_MAKE_CD_CASE
(
SurfaceMeshToSurfaceMeshCD
,
SurfaceMesh
,
SurfaceMesh
),
IMSTK_MAKE_CD_CASE
(
SurfaceMeshToCapsuleCD
,
SurfaceMesh
,
Capsule
),
IMSTK_MAKE_CD_CASE
(
SurfaceMeshToSphereCD
,
SurfaceMesh
,
Sphere
),
IMSTK_MAKE_CD_CASE
(
TetraToPointSetCD
,
TetrahedralMesh
,
PointSet
),
IMSTK_MAKE_CD_CASE
(
UnidirectionalPlaneToSphereCD
,
Plane
,
Sphere
)
...
...
Write
Preview
Supports
Markdown
0%
Try again
or
attach a new file
.
Cancel
You are about to add
0
people
to the discussion. Proceed with caution.
Finish editing this message first!
Cancel
Please
register
or
sign in
to comment