Skip to content

GitLab

Commit bdbffbb8 authored by Andrew Wilson's avatar Andrew Wilson
Browse files

REFAC: Camera now provides projection allowing picking, RbdPlaneToSphereCDExample now uses picking

parent ed212e8f
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<RigidObject2> rbdObj2("rbdObj2");
// Collision Interaction between rigid objects
{
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
......
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