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
bdbffbb8
Commit
bdbffbb8
authored
Aug 05, 2021
by
Andrew Wilson
🐘
Browse files
REFAC: Camera now provides projection allowing picking, RbdPlaneToSphereCDExample now uses picking
parent
ed212e8f
Changes
5
Hide whitespace changes
Inline
Side-by-side
Examples/RBD/RbdPlaneToSphereCD/RbdPlaneToSphereCDExample.cpp
View file @
bdbffbb8
...
...
@@ -39,6 +39,8 @@
#include
"imstkSphere.h"
#include
"imstkVisualModel.h"
#include
"imstkVTKViewer.h"
#include
"imstkMouseDeviceClient.h"
#include
"imstkCollisionUtils.h"
using
namespace
imstk
;
...
...
@@ -53,6 +55,7 @@ main()
Logger
::
startLogger
();
imstkNew
<
Scene
>
scene
(
"RbdMeshMeshCollision"
);
scene
->
getConfig
()
->
taskParallelizationEnabled
=
false
;
// This model is shared among interacting rigid bodies
imstkNew
<
RigidBodyModel2
>
rbdModel
;
...
...
@@ -73,59 +76,52 @@ main()
scene
->
addSceneObject
(
planeObj
);
}
// Create surface mesh cube (so we can use pointset for point->implicit collision)
imstkNew
<
RigidObject2
>
rbdObj1
(
"rbdObj1"
);
std
::
shared_ptr
<
RigidObject2
>
rigidObjects
[]
=
{
nullptr
,
nullptr
,
nullptr
,
nullptr
};
for
(
int
i
=
0
;
i
<
4
;
i
++
)
{
imstkNew
<
Sphere
>
sphere
(
Vec3d
(
0.0
,
0.0
,
0.0
),
1.0
);
rigidObjects
[
i
]
=
std
::
make_shared
<
RigidObject2
>
(
"RbdObject"
+
std
::
to_string
(
i
));
imstkNew
<
Sphere
>
sphere
(
Vec3d
::
Zero
(),
0.8
);
// Create the cube rigid object
rbdObj1
->
setDynamicalModel
(
rbdModel
);
rbdObj1
->
setPhysicsGeometry
(
sphere
);
rbdObj1
->
setCollidingGeometry
(
sphere
);
rbdObj1
->
setVisualGeometry
(
sphere
);
rbdObj1
->
getRigidBody
()
->
m_mass
=
1.0
;
rbdObj1
->
getRigidBody
()
->
m_initPos
=
Vec3d
(
0.0
,
8.0
,
0.0
);
rbdObj1
->
getRigidBody
()
->
m_intertiaTensor
=
Mat3d
::
Identity
();
scene
->
addSceneObject
(
rbdObj1
);
rigidObjects
[
i
]
->
setDynamicalModel
(
rbdModel
);
rigidObjects
[
i
]
->
setPhysicsGeometry
(
sphere
);
rigidObjects
[
i
]
->
setCollidingGeometry
(
sphere
);
rigidObjects
[
i
]
->
setVisualGeometry
(
sphere
);
rigidObjects
[
i
]
->
getRigidBody
()
->
m_mass
=
1.0
;
const
double
t
=
static_cast
<
double
>
(
i
)
/
4.0
;
rigidObjects
[
i
]
->
getRigidBody
()
->
m_initPos
=
Vec3d
(
t
*
8.0
-
3.0
,
1.0
,
0.0
);
rigidObjects
[
i
]
->
getRigidBody
()
->
m_intertiaTensor
=
Mat3d
::
Identity
();
rigidObjects
[
i
]
->
getVisualModel
(
0
)
->
getRenderMaterial
()
->
setColor
(
Color
::
lerpRgb
(
Color
::
Red
,
Color
::
Blue
,
t
));
scene
->
addSceneObject
(
rigidObjects
[
i
]);
}
imstkNew
<
R
igid
O
bject
2
>
rbdObj2
(
"rbdObj2"
);
// Collision Interaction between r
igid
o
bject
s
{
imstkNew
<
Sphere
>
sphere
(
Vec3d
(
0.0
,
0.0
,
0.0
),
1.0
);
// Create the cube rigid object
rbdObj2
->
setDynamicalModel
(
rbdModel
);
rbdObj2
->
setPhysicsGeometry
(
sphere
);
rbdObj2
->
setCollidingGeometry
(
sphere
);
rbdObj2
->
setVisualGeometry
(
sphere
);
rbdObj2
->
getRigidBody
()
->
m_mass
=
1.0
;
rbdObj2
->
getRigidBody
()
->
m_initPos
=
Vec3d
(
0.2
,
1.0
,
0.0
);
rbdObj2
->
getRigidBody
()
->
m_intertiaTensor
=
Mat3d
::
Identity
();
scene
->
addSceneObject
(
rbdObj2
);
}
for
(
int
i
=
0
;
i
<
4
;
i
++
)
{
auto
rbdInteraction
=
std
::
make_shared
<
RigidObjectCollision
>
(
rigidObjects
[
i
],
planeObj
,
"UnidirectionalPlaneToSphereCD"
);
rbdInteraction
->
setFriction
(
0.0
);
rbdInteraction
->
setStiffness
(
0.0001
);
scene
->
getCollisionGraph
()
->
addInteraction
(
rbdInteraction
);
}
// Collision Interaction between plane and rbdobj1
{
auto
rbdInteraction1
=
std
::
make_shared
<
RigidObjectCollision
>
(
rbdObj1
,
planeObj
,
"UnidirectionalPlaneToSphereCD"
);
rbdInteraction1
->
setFriction
(
0.0
);
rbdInteraction1
->
setStiffness
(
0.0001
);
scene
->
getCollisionGraph
()
->
addInteraction
(
rbdInteraction1
);
auto
rbdInteraction2
=
std
::
make_shared
<
RigidObjectCollision
>
(
rbdObj2
,
planeObj
,
"UnidirectionalPlaneToSphereCD"
);
rbdInteraction2
->
setFriction
(
0.0
);
rbdInteraction2
->
setStiffness
(
0.0001
);
scene
->
getCollisionGraph
()
->
addInteraction
(
rbdInteraction2
);
auto
rbdInteraction3
=
std
::
make_shared
<
RigidObjectCollision
>
(
rbdObj1
,
rbdObj2
,
"SphereToSphereCD"
);
rbdInteraction3
->
setFriction
(
0.0
);
rbdInteraction3
->
setStiffness
(
0.0001
);
scene
->
getCollisionGraph
()
->
addInteraction
(
rbdInteraction3
);
for
(
int
i
=
0
;
i
<
4
;
i
++
)
{
for
(
int
j
=
i
+
1
;
j
<
4
;
j
++
)
{
auto
rbdInteraction
=
std
::
make_shared
<
RigidObjectCollision
>
(
rigidObjects
[
i
],
rigidObjects
[
j
],
"SphereToSphereCD"
);
rbdInteraction
->
setFriction
(
0.0
);
rbdInteraction
->
setStiffness
(
0.0001
);
scene
->
getCollisionGraph
()
->
addInteraction
(
rbdInteraction
);
}
}
}
// Camera
scene
->
getActiveCamera
()
->
setPosition
(
0.0
,
40.0
,
40.0
);
scene
->
getActiveCamera
()
->
setPosition
(
0.0252374
,
2.85008
,
17.0338
);
scene
->
getActiveCamera
()
->
setFocalPoint
(
0.30457
,
2.99155
,
0.24512
);
scene
->
getActiveCamera
()
->
setViewUp
(
0.0016057
,
0.999996
,
0.00220191
);
// Light
imstkNew
<
DirectionalLight
>
light
;
...
...
@@ -161,53 +157,75 @@ main()
viewer
->
addControl
(
keyControl
);
}
// The following implements pick and drag controls for a sphere, this could be more elegantly
// implemented in a subclass of MouseControl
LOG
(
INFO
)
<<
"RbdObj Controls:"
;
LOG
(
INFO
)
<<
"----------------------------------------------------------------------"
;
LOG
(
INFO
)
<<
" | i - forward movement"
;
LOG
(
INFO
)
<<
" | j - left movement"
;
LOG
(
INFO
)
<<
" | l - right movement"
;
LOG
(
INFO
)
<<
" | k - backwards movement"
;
LOG
(
INFO
)
<<
" | u - rotate left"
;
LOG
(
INFO
)
<<
" | o - rotate right"
;
// Not perfectly thread safe movement lambda, ijkl movement instead of wasd because d is already used
std
::
shared_ptr
<
KeyboardDeviceClient
>
keyDevice
=
viewer
->
getKeyboardDevice
();
connect
<
Event
>
(
sceneManager
,
&
SceneManager
::
postUpdate
,
[
&
](
Event
*
)
LOG
(
INFO
)
<<
" | click and drag to pick up sphere"
;
// We do picking/dragging of sphere's by implementing movement on the plane whose normal is the
// view direction and center is the sphere position when picked up
int
sphereSelected
=
-
1
;
Vec3d
planePos
;
// Perform picking on to figure out which sphere was clicked
connect
<
MouseEvent
>
(
viewer
->
getMouseDevice
(),
&
MouseDeviceClient
::
mouseButtonPress
,
[
&
](
MouseEvent
*
e
)
{
// Keep cube updating at real time
const
double
dt
=
rbdObj1
->
getRigidBodyModel2
()
->
getConfig
()
->
m_dt
=
sceneManager
->
getDt
();
Vec3d
extForce
=
Vec3d
(
0.0
,
0.0
,
0.0
);
Vec3d
extTorque
=
Vec3d
(
0.0
,
0.0
,
0.0
);
// If i down, move forward
if
(
keyDevice
->
getButton
(
'i'
)
==
KEY_PRESS
)
if
(
e
->
m_buttonId
==
0
)
{
extForce
+=
Vec3d
(
0.0
,
0.0
,
-
1500.0
)
*
dt
;
// Get mouse position (0, 1) with origin at bot left of screen
const
Vec2d
mousePos
=
viewer
->
getMouseDevice
()
->
getPos
();
// To NDC coordinates
const
Vec3d
rayDir
=
scene
->
getActiveCamera
()
->
getEyeRayDir
(
Vec2d
(
mousePos
[
0
]
*
2.0
-
1.0
,
mousePos
[
1
]
*
2.0
-
1.0
));
const
Vec3d
rayStart
=
scene
->
getActiveCamera
()
->
getPosition
();
for
(
int
i
=
0
;
i
<
4
;
i
++
)
{
auto
sphere
=
std
::
dynamic_pointer_cast
<
Sphere
>
(
rigidObjects
[
i
]
->
getPhysicsGeometry
());
Vec3d
iPt
;
if
(
CollisionUtils
::
testRayToSphere
(
rayStart
,
rayDir
,
sphere
->
getCenter
(),
sphere
->
getRadius
(),
iPt
))
{
sphereSelected
=
i
;
planePos
=
sphere
->
getCenter
();
}
}
}
if
(
keyDevice
->
getButton
(
'k'
)
==
KEY_PRESS
)
{
extForce
+=
Vec3d
(
0.0
,
0.0
,
1500.0
)
*
dt
;
}
if
(
keyDevice
->
getButton
(
'j'
)
==
KEY_PRESS
)
{
extForce
+=
Vec3d
(
-
1500.0
,
0.0
,
0.0
)
*
dt
;
}
if
(
keyDevice
->
getButton
(
'l'
)
==
KEY_PRESS
)
{
extForce
+=
Vec3d
(
1500.0
,
0.0
,
0.0
)
*
dt
;
}
if
(
keyDevice
->
getButton
(
'u'
)
==
KEY_PRESS
)
});
// Unselect/drop the sphere
connect
<
MouseEvent
>
(
viewer
->
getMouseDevice
(),
&
MouseDeviceClient
::
mouseButtonRelease
,
[
&
](
MouseEvent
*
e
)
{
if
(
e
->
m_buttonId
==
0
)
{
extTorque
+=
Vec3d
(
0.0
,
1.5
,
0.0
)
*
dt
;
sphereSelected
=
-
1
;
}
if
(
keyDevice
->
getButton
(
'o'
)
==
KEY_PRESS
)
});
connect
<
Event
>
(
sceneManager
,
&
SceneManager
::
postUpdate
,
[
&
](
Event
*
)
{
// Keep cube updating at real time
std
::
shared_ptr
<
RigidBodyModel2
>
rbdModel
=
rigidObjects
[
0
]
->
getRigidBodyModel2
();
// All bodies share a model
const
double
dt
=
rbdModel
->
getConfig
()
->
m_dt
=
sceneManager
->
getDt
();
if
(
sphereSelected
!=
-
1
)
{
extTorque
+=
Vec3d
(
0.0
,
-
1.5
,
0.0
)
*
dt
;
// Get mouses current position
const
Vec2d
mousePos
=
viewer
->
getMouseDevice
()
->
getPos
();
const
Vec3d
rayDir
=
scene
->
getActiveCamera
()
->
getEyeRayDir
(
Vec2d
(
mousePos
[
0
]
*
2.0
-
1.0
,
mousePos
[
1
]
*
2.0
-
1.0
));
const
Vec3d
rayStart
=
scene
->
getActiveCamera
()
->
getPosition
();
// Exert a force to bring it to the mouse position on the plane
auto
sphere
=
std
::
dynamic_pointer_cast
<
Sphere
>
(
rigidObjects
[
sphereSelected
]
->
getPhysicsGeometry
());
Vec3d
iPt
;
CollisionUtils
::
testRayToPlane
(
rayStart
,
rayDir
,
planePos
,
scene
->
getActiveCamera
()
->
getForward
(),
iPt
);
const
Vec3d
fS
=
(
iPt
-
sphere
->
getPosition
())
*
100.0
;
// Spring force
const
Vec3d
fD
=
-
rigidObjects
[
sphereSelected
]
->
getRigidBody
()
->
getVelocity
()
*
10.0
;
// Spring damping
*
rigidObjects
[
sphereSelected
]
->
getRigidBody
()
->
m_force
+=
(
fS
+
fD
);
}
*
rbdObj1
->
getRigidBody
()
->
m_force
=
extForce
;
*
rbdObj1
->
getRigidBody
()
->
m_torque
=
extTorque
;
});
});
driver
->
start
();
}
...
...
Source/CollisionDetection/CollisionDetection/imstkCollisionUtils.h
View file @
bdbffbb8
...
...
@@ -908,6 +908,56 @@ testRayToObb(const Vec3d& rayOrigin, const Vec3d& rayDir,
return
true
;
}
///
/// \brief ray sphere intersection with intersection point
/// Real time collision detection 2004
///
inline
bool
testRayToSphere
(
const
Vec3d
&
rayOrigin
,
const
Vec3d
&
rayDir
,
const
Vec3d
&
spherePos
,
const
double
radius
,
Vec3d
&
iPt
)
{
const
Vec3d
m
=
rayOrigin
-
spherePos
;
double
b
=
m
.
dot
(
rayDir
);
double
c
=
m
.
dot
(
m
)
-
radius
*
radius
;
// Exit if r’s origin outside s (c > 0) and r pointing away from s (b > 0)
if
(
c
>
0.0
&&
b
>
0.0
)
{
return
false
;
}
double
discr
=
b
*
b
-
c
;
// A negative discriminant corresponds to ray missing sphere
if
(
discr
<
0.0
)
{
return
false
;
}
// Ray now found to intersect sphere, compute smallest t value of intersection
// If t is negative, ray started inside sphere so clamp t to zero
double
t
=
std
::
max
(
0.0
,
-
b
-
std
::
sqrt
(
discr
));
iPt
=
rayOrigin
+
t
*
rayDir
;
return
true
;
}
///
/// \brief Ray plane intersection with intersection point
///
inline
bool
testRayToPlane
(
const
Vec3d
&
rayOrigin
,
const
Vec3d
&
rayDir
,
const
Vec3d
&
planePos
,
const
Vec3d
&
planeNormal
,
Vec3d
&
iPt
)
{
const
double
denom
=
rayDir
.
dot
(
planeNormal
);
// Plane and ray are parallel
if
(
std
::
abs
(
denom
)
<
IMSTK_DOUBLE_EPS
)
{
return
false
;
}
const
double
t
=
(
planePos
-
rayOrigin
).
dot
(
planeNormal
)
/
denom
;
iPt
=
rayOrigin
+
t
*
rayDir
;
return
true
;
}
///
/// \brief Compute closest distance from a point to a segment x1-x2
///
...
...
Source/Rendering/VTKRenderer/imstkVTKRenderer.cpp
View file @
bdbffbb8
...
...
@@ -417,6 +417,16 @@ VTKRenderer::updateCamera()
m_camera
->
SetViewUp
(
up
[
0
],
up
[
1
],
up
[
2
]);
m_camera
->
SetViewAngle
(
cam
->
getFieldOfView
());
m_camera
->
SetClippingRange
(
cam
->
getNearZ
(),
cam
->
getFarZ
());
// Copy the projection back to the camera
vtkMatrix4x4
*
projVtk
=
m_camera
->
GetProjectionTransformMatrix
(
m_vtkRenderer
);
for
(
int
i
=
0
;
i
<
4
;
i
++
)
{
for
(
int
j
=
0
;
j
<
4
;
j
++
)
{
cam
->
m_proj
(
i
,
j
)
=
projVtk
->
GetElement
(
j
,
i
);
}
}
}
void
...
...
Source/SceneEntities/Camera/imstkCamera.h
View file @
bdbffbb8
...
...
@@ -76,22 +76,35 @@ public:
Camera
()
=
default
;
virtual
~
Camera
()
=
default
;
///
/// \brief Allows setting of the projection matrix without exposing a setter
/// only renders may set it
///
friend
class
VTKRenderer
;
public:
///
/// \brief Get camera view matrix
/// \returns Camera view matrix reference
///
inline
Mat4d
&
getView
()
{
return
m_view
;
}
Mat4d
&
getView
()
{
return
m_view
;
}
///
/// \brief Get camera projection matrix, this matrix will
/// be identity until first render is done
/// \returns Camera projection matrix reference
///
Mat4d
&
getProj
()
{
return
m_proj
;
}
///
/// \brief Get the inverse view matrix
///
inline
const
Mat4d
&
getInvView
()
{
return
m_invView
;
}
const
Mat4d
&
getInvView
()
{
return
m_invView
;
}
///
/// \brief Set the camera view matrix
///
inline
void
setView
(
const
Mat4d
&
view
)
void
setView
(
const
Mat4d
&
view
)
{
m_viewModified
=
false
;
m_view
=
view
;
...
...
@@ -102,13 +115,13 @@ public:
/// \brief Gets the field of view
/// \returns vertical field of view in degrees
///
inline
const
double
getFieldOfView
()
const
{
return
m_fieldOfView
;
}
const
double
getFieldOfView
()
const
{
return
m_fieldOfView
;
}
///
/// \brief Sets the field of view
/// \param vertical field of view in degrees
///
inline
void
setFieldOfView
(
const
double
fov
)
void
setFieldOfView
(
const
double
fov
)
{
m_fieldOfView
=
fov
;
//m_projModified = true;
...
...
@@ -119,19 +132,19 @@ public:
/// note: You lose depth accuracy as the range between near and far increases
/// could cause z fighting
///
inline
void
setNearZ
(
const
double
nearZ
)
{
m_nearZ
=
nearZ
;
}
inline
const
double
getNearZ
()
const
{
return
m_nearZ
;
}
void
setNearZ
(
const
double
nearZ
)
{
m_nearZ
=
nearZ
;
}
const
double
getNearZ
()
const
{
return
m_nearZ
;
}
///
/// \brief Set clipping near
/// note: You lose depth accuracy as the range between near and far increases
/// could cause z fighting
///
inline
void
setFarZ
(
const
double
farZ
)
{
m_farZ
=
farZ
;
}
void
setFarZ
(
const
double
farZ
)
{
m_farZ
=
farZ
;
}
inline
const
double
getFarZ
()
const
{
return
m_farZ
;
}
const
double
getFarZ
()
const
{
return
m_farZ
;
}
inline
virtual
void
update
()
virtual
void
update
()
{
if
(
m_viewModified
)
{
...
...
@@ -149,20 +162,20 @@ public:
/// \brief Gets the camera position
/// \returns camera position
///
inline
const
Vec3d
&
getPosition
()
const
{
return
m_position
;
}
const
Vec3d
&
getPosition
()
const
{
return
m_position
;
}
///
/// \brief Sets the camera position
///
inline
void
setPosition
(
const
Vec3d
&
pos
)
void
setPosition
(
const
Vec3d
&
pos
)
{
m_position
=
pos
;
m_viewModified
=
true
;
}
inline
void
setPosition
(
const
double
x
,
const
double
y
,
const
double
z
)
void
setPosition
(
const
double
x
,
const
double
y
,
const
double
z
)
{
setPosition
(
Vec3d
(
x
,
y
,
z
));
}
...
...
@@ -172,42 +185,56 @@ public:
/// The focal point is the point that the camera points to
/// \returns Focal point position
///
inline
const
Vec3d
&
getFocalPoint
()
const
{
return
m_focalPoint
;
}
const
Vec3d
&
getFocalPoint
()
const
{
return
m_focalPoint
;
}
///
/// \brief Sets the
focal point
/// \brief Sets the
point to look at, point the camera towards
///
inline
void
setFocalPoint
(
const
Vec3d
&
focalPt
)
void
setFocalPoint
(
const
Vec3d
&
focalPt
)
{
m_focalPoint
=
focalPt
;
m_viewModified
=
true
;
}
inline
void
setFocalPoint
(
const
double
x
,
const
double
y
,
const
double
z
)
void
setFocalPoint
(
const
double
x
,
const
double
y
,
const
double
z
)
{
setFocalPoint
(
Vec3d
(
x
,
y
,
z
));
}
///
/// \brief Get the up vector
/// \returns up vector of camera
/// \brief Get the up direction of the view
///
const
Vec3d
&
getViewUp
()
const
{
return
m_viewUp
;
}
///
/// \brief Get the forward/look direction of the view
///
const
Vec3d
&
getForward
()
const
{
return
m_view
.
col
(
2
).
head
<
3
>
();
}
///
inline
const
Vec3d
&
getViewUp
()
const
{
return
m_viewUp
;
}
/// \brief Compute ray emanating from the camera position that travels
/// through the point in normalized device coordinates (-1,1 on x and y view plane)
///
const
Vec3d
getEyeRayDir
(
const
Vec2d
&
ndcPos
)
const
{
const
Vec4d
worldPos
=
(
m_proj
*
m_view
).
inverse
()
*
Vec4d
(
ndcPos
[
0
],
ndcPos
[
1
],
0.0
,
1.0
);
return
(
worldPos
.
head
<
3
>
()
/
worldPos
[
3
]
-
m_position
).
normalized
();
}
///
/// \brief Set the up vector
///
inline
void
setViewUp
(
const
Vec3d
&
up
)
void
setViewUp
(
const
Vec3d
&
up
)
{
m_viewUp
=
up
.
normalized
();
m_viewModified
=
true
;
}
inline
void
setViewUp
(
const
double
x
,
const
double
y
,
const
double
z
)
void
setViewUp
(
const
double
x
,
const
double
y
,
const
double
z
)
{
setViewUp
(
Vec3d
(
x
,
y
,
z
));
}
...
...
@@ -221,9 +248,8 @@ protected:
// Base camera values
Mat4d
m_view
=
Mat4d
::
Identity
();
///> Actual view matrix used
Mat4d
m_invView
=
Mat4d
::
Identity
();
///> Inverse is often needed so we maintain it
//Mat4d m_proj;
bool
m_viewModified
=
true
;
//bool m_projModified = true;
Mat4d
m_proj
;
///> Only modifiable through projection parameters (fov,near,far)
bool
m_viewModified
=
true
;
// Base projection parameters
double
m_fieldOfView
=
40.0
;
///> field of view in degrees
...
...
Source/SimulationManager/imstkViewer.h
View file @
bdbffbb8
...
...
@@ -81,6 +81,9 @@ public:
/// \brief Set the render window size
///
virtual
void
setSize
(
int
,
int
)
{
}
void
setSize
(
Vec2i
size
)
{
setSize
(
size
[
0
],
size
[
1
]);
}
virtual
const
Vec2i
getSize
()
const
{
return
Vec2i
::
Zero
();
}
///
/// \brief Get the current renderer's mode
...
...
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