AnimatLab  2
Test
VsCameraManipulator.cpp
1 #include "StdAfx.h"
2 
3 #include "VsMouseSpring.h"
4 #include "VsCameraManipulator.h"
5 #include "VsMovableItem.h"
6 #include "VsBody.h"
7 #include "VsJoint.h"
8 #include "VsRigidBody.h"
9 #include "VsStructure.h"
10 #include "VsLight.h"
11 #include "VsSimulator.h"
12 #include "VsOsgUserData.h"
13 #include "VsDragger.h"
14 
15 namespace VortexAnimatSim
16 {
17  namespace Visualization
18  {
19 
20 VsCameraManipulator::VsCameraManipulator(Simulator *lpSim, osgViewer::Viewer *osgViewer, osg::Viewport *osgViewport)
21 {
22  m_lpSim = lpSim;
23  m_osgViewer = osgViewer;
24  m_osgViewport = osgViewport;
25  m_lpPicked = NULL;
26 
27  m_bShiftDown = false;
28  m_bControlDown = false;
29  m_bInDrag = false;
30 
31  m_fltPrevX = 0;
32  m_fltPrevY = 0;
33 
34  m_v3Eye.set(0,0, 10);
35 
36  //Set this very small to prevent it from keeping you from zooming in as close as you need to for a part.
37  this->setMinimumZoomScale(0.00001);
38 }
39 
40 VsCameraManipulator::~VsCameraManipulator(void)
41 {
42  VsMouseSpring::GetInstance()->SetRigidBody(NULL);
43 }
44 
45 
46 bool VsCameraManipulator::handle(const GUIEventAdapter& ea, GUIActionAdapter& aa)
47 {
48  bool bHandled = false;
49 
50  try
51  {
52  float x = ea.getX();
53  float y = ea.getY();
54 
55  //Take an action based on the event type
56  switch(ea.getEventType())
57  {
58  case(GUIEventAdapter::PUSH):
59  {
60  pick(ea, aa);
61  break;
62  }
63 
64  case(GUIEventAdapter::RELEASE):
65  {
66  VsMouseSpring::GetInstance()->Visible(false);
67 
68  if(!m_bInDrag)
69  {
70  if(m_lpPicked && m_lpSim && m_lpSim->VisualSelectionMode() != SIMULATION_SELECTION_MODE)
71  {
72  //Change the selected vertex of the selected part.
73  m_lpPicked->SelectedVertex(m_vSelectedVertex.x(), m_vSelectedVertex.y(), m_vSelectedVertex.z(), true, true);
74 
75  if(m_lpSim->AddBodiesMode())
76  m_lpPicked->AddBodyClicked(m_vPickPoint.x(), m_vPickPoint.y(), m_vPickPoint.z(),
77  m_vPickNormal.x(), m_vPickNormal.y(), m_vPickNormal.z());
78  else
79  m_lpPicked->Selected(true, m_bControlDown);
80  }
81  }
82 
83  m_bInDrag = false;
84  break;
85  }
86 
87  case(GUIEventAdapter::DRAG):
88  {
89 
90  m_bInDrag = true;
91  //if both left control and left shift are pressed then
92  //do the mouse spring
93  if(m_bShiftDown && m_bControlDown && CanDoMouseSpring())
94  bHandled = DoMouseSpring(ea, x, y);
95  //if just left shift is pressed then do pan
96  else if(m_bShiftDown)
97  {
98  DoPan(ea, x, y);
99  bHandled = true;
100  }
102  //else if(m_bLeftControl)
103  //{
104  // DoZoom(ea, x, y);
105  //}
107  //else
108  //{
109  // DoRotate(ea, x, y);
110  //}
111  break;
112  }
113 
114  case(GUIEventAdapter::KEYDOWN):
115  {
116  //std::string strText = "Key Down" + STR(ea.getKey());
117  //OutputDebugString(strText.c_str());
118  if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Shift_L)
119  {
120  m_bShiftDown = true;
121  //bHandled = true;
122  }
123 
124  if(ea.getKey() == osgGA::GUIEventAdapter::KEY_Control_L)
125  {
126  m_bControlDown = true;
127  //bHandled = true;
128  }
129 
130  if(m_bInDrag && m_bShiftDown && m_bControlDown)
131  {
132  bHandled = DoMouseSpring(ea, x, y);
133  //bHandled = true;
134  }
135  break;
136  }
137 
138  case(GUIEventAdapter::KEYUP):
139  {
140  //std::string strText = "Key Up" + STR(ea.getKey());
141  //OutputDebugString(strText.c_str());
142  if (ea.getKey() == osgGA::GUIEventAdapter::KEY_Shift_L)
143  {
144  m_bShiftDown = false;
145  //bHandled = true;
146  }
147 
148  if(ea.getKey() == osgGA::GUIEventAdapter::KEY_Control_L)
149  {
150  m_bControlDown = false;
151  //bHandled = true;
152  }
153 
154  if(ea.getKey() == osgGA::GUIEventAdapter::KEY_F10 && m_lpSim)
155  m_lpSim->ResetSimulation();
156 
157  if(ea.getKey() == osgGA::GUIEventAdapter::KEY_F5 && m_lpSim)
158  m_lpSim->ToggleSimulation();
159 
160  break;
161  }
162  default:
163  bHandled = false;
164  }//end switch statement
165 
166  if(!bHandled)
167  bHandled = osgGA::TrackballManipulator::handle(ea, aa);
168 
169  m_fltPrevX = x;
170  m_fltPrevY = y;
171 
172  } //Eat any erorrs here.
173  catch(CStdErrorInfo oError)
174  {return false;}
175  catch(...)
176  {return false;}
177 
178  return bHandled;
179 }
180 
181 const osg::Camera *VsCameraManipulator::GetCamera(float x, float y, float &local_x, float &local_y)
182 {
183  const osg::Camera* camera = m_osgViewer->getCameraContainingPosition(x, y, local_x, local_y);
184  if (!camera) camera = m_osgViewer->getCamera();
185  return camera;
186 }
187 
188 const osg::Camera *VsCameraManipulator::GetCamera(float x, float y)
189 {
190  float local_x=0, local_y=0;
191  const osg::Camera* camera = m_osgViewer->getCameraContainingPosition(x, y, local_x, local_y);
192  if (!camera) camera = m_osgViewer->getCamera();
193  return camera;
194 }
195 
196 void VsCameraManipulator::pick(const osgGA::GUIEventAdapter& ea, GUIActionAdapter& aa)
197 {
198  osgUtil::LineSegmentIntersector::Intersections intersections;
199 
200  m_lpPicked = NULL;
201 
202  //We have clicked something else so set the rigid body to null.
203  VsMouseSpring::GetInstance()->SetRigidBody(NULL);
204 
205  float x = ea.getX();
206  float y = ea.getY();
207 
208  float local_x, local_y = 0.0;
209  const osg::Camera* camera = GetCamera(x, y, local_x, local_y);
210 
211  osgUtil::LineSegmentIntersector::CoordinateFrame cf = camera->getViewport() ? osgUtil::Intersector::WINDOW : osgUtil::Intersector::PROJECTION;
212  osg::ref_ptr< osgUtil::LineSegmentIntersector > picker = new osgUtil::LineSegmentIntersector(cf, local_x, local_y);
213 
214  RigidBody *lpBody = NULL;
215  Joint *lpJoint = NULL;
216  Structure *lpStruct = NULL;
217  Light *lpLight = NULL;
218  if (m_osgViewer->computeIntersections(x,y,intersections))
219  {
220 
221 #pragma region LineSegmentIntersector
222 
223  for(osgUtil::LineSegmentIntersector::Intersections::iterator hitr = intersections.begin();
224  hitr != intersections.end();
225  ++hitr)
226  {
227  //Look for the first drawable object that has a defined user data.
228  if (hitr->drawable.valid() && hitr->drawable->getUserData())
229  {
230  //If the user data is a VsOsgUserData object then lets use it.
231  VsOsgUserData *osgData = dynamic_cast<VsOsgUserData *>(hitr->drawable->getUserData());
232 
233  if(osgData && m_lpSim)
234  {
235  lpBody = osgData->GetBody();
236  lpJoint = osgData->GetJoint();
237  lpStruct = osgData->GetStructure();
238  lpLight = osgData->GetLight();
239 
240  m_vPickPoint = hitr->getWorldIntersectPoint();
241  m_vPickNormal = hitr->getWorldIntersectNormal();
242  m_vSelectedVertex = FindSelectedVertex(hitr);
243 
244  switch (m_lpSim->VisualSelectionMode())
245  {
246  case GRAPHICS_SELECTION_MODE:
247  case COLLISION_SELECTION_MODE:
248  //if(lpBody)
249  //{ //For debugging.
250  // bool bManip = lpBody->AllowMouseManipulation();
251  // bool bVisible = lpBody->IsVisible();
252  // int iType = lpBody->VisualSelectionType();
253  // int iMode = m_lpSim->VisualSelectionMode();
254  // iMode=iType;
255  //}
256 
257  if(lpBody && lpBody->AllowMouseManipulation() && lpBody->IsVisible() && (lpBody->VisualSelectionType() & m_lpSim->VisualSelectionMode()) )
258  {
259  m_lpPicked = lpBody;
260  return;
261  }
262  if(lpStruct && lpStruct->IsVisible() )
263  {
264  m_lpPicked = lpStruct;
265  return;
266  }
267  if(lpLight && lpLight->IsVisible() )
268  {
269  m_lpPicked = lpLight;
270  return;
271  }
272  break;
273 
274  case JOINT_SELECTION_MODE:
275  if(lpJoint && lpJoint->AllowMouseManipulation() && lpJoint->IsVisible() && (lpJoint->VisualSelectionType() & m_lpSim->VisualSelectionMode()) )
276  {
277  m_lpPicked = lpJoint;
278  return;
279  }
280  break;
281 
282  case RECEPTIVE_FIELD_SELECTION_MODE:
283  if(lpBody && lpBody->AllowMouseManipulation() && lpBody->IsVisible() && (lpBody->VisualSelectionType() == COLLISION_SELECTION_MODE) )
284  {
285  m_lpPicked = lpBody;
286  return;
287  }
288  break;
289 
290  case SIMULATION_SELECTION_MODE:
291  if(lpBody && lpBody->AllowMouseManipulation() && lpBody->IsVisible() && (lpBody->VisualSelectionType() & m_lpSim->VisualSelectionMode()) )
292  {
293  VsMouseSpring::GetInstance()->SetRigidBody(osgData->GetVsBody());
294  VsMouseSpring::GetInstance()->SetGrabPosition(hitr->getLocalIntersectPoint());
295  m_lpPicked = osgData->GetBody();
296  return;
297  }
298  break;
299 
300  default:
301  break;
302  }
303  }
304  }
305  }
306 #pragma endregion
307 
308  }
309 
310  //If nothing was picked then set the mouse spring body to null
311  VsMouseSpring::GetInstance()->SetRigidBody(NULL);
312  m_lpPicked = NULL;
313 }
314 
315 osg::Vec3 VsCameraManipulator::FindSelectedVertex(osgUtil::LineSegmentIntersector::Intersections::iterator &hitr)
316 {
317  //Get the selected vertex.
318  const osgUtil::LineSegmentIntersector::Intersection::IndexList& vil = hitr->indexList;
319  const osgUtil::LineSegmentIntersector::Intersection::RatioList& ral = hitr->ratioList;
320  osg::Geometry *osgGeom = dynamic_cast<osg::Geometry *>(hitr->drawable.get());
321  if(vil.size() > 0 && osgGeom)
322  {
323  //First get the vertex array.
324  osg::ref_ptr<osg::Vec3Array> aryVert = dynamic_cast<osg::Vec3Array*>(osgGeom->getVertexArray());
325 
326  //Then we need to find the vertex that was closest to the hit. We do this by first interpolating the
327  //hit position using the barycentric coords. Then find the vertex that is closest to that point.
328  if(vil.size() != ral.size())
329  THROW_ERROR(Vs_Err_lHitArrayMismatch, Vs_Err_strHitArrayMismatch);
330 
331  int iSize = vil.size();
332  osg::Vec3 osgHit(0, 0, 0);
333  for(int iIdx=0; iIdx<iSize; iIdx++)
334  {
335  osgHit += ((*aryVert)[vil[iIdx]])*ral[iIdx];
336  }
337 
338  double dblMin = 10000, dblDist = 0;
339  osg::Vec3 vSelVert;
340  for(int iIdx=0; iIdx<iSize; iIdx++)
341  {
342  osg::Vec3 osgVert = (*aryVert)[vil[iIdx]];
343  dblDist = Std_CalculateDistance(osgHit.x(), osgHit.y(), osgHit.z(), osgVert.x(), osgVert.y(), osgVert.z());
344  if(dblDist < dblMin)
345  {
346  dblMin = dblDist;
347  vSelVert = osgVert;
348  }
349  }
350 
351  return vSelVert;
352  }
353  else
354  return osg::Vec3(0, 0, 0);
355 
356 }
357 
358 /*
359 void VsCameraManipulator::GetPickedFace(int iIndex, osg::Geometry *osgGeo)
360 {
361  osg::Vec3Array *aryVerts = (osg::Vec3Array *) geo->getVertexArray();
362  iSize = aryVerts->getNumElements();
363  if(iIndex >=0 && iIndex < iSize)
364  {
365  m_vPickedFace = (osg::Vec3) aryVerts->at(iIndex);
366 
367  }
368 }
369 */
370 
371 bool VsCameraManipulator::CanDoMouseSpring()
372 {
373  VsRigidBody *osgRBBody = VsMouseSpring::GetInstance()->GetRigidBody();
374 
375  if (!osgRBBody)
376  return false;
377 
378  RigidBody *rbBody = dynamic_cast<RigidBody *>(osgRBBody);
379 
380  if (!rbBody)
381  return false;
382 
383  return true;
384 }
385 
386 bool VsCameraManipulator::DoMouseSpring(const GUIEventAdapter& ea, float x, float y)
387 {
388  VsMouseSpring::GetInstance()->Visible(false);
389 
390  VsRigidBody *osgRBBody = VsMouseSpring::GetInstance()->GetRigidBody();
391 
392  if (!osgRBBody)
393  return false;
394 
395  RigidBody *rbBody = dynamic_cast<RigidBody *>(osgRBBody);
396 
397  if (!rbBody)
398  return false;
399 
400  VsMouseSpring::GetInstance()->Visible(true);
401 
402  //char strDest[150];
403 
404  //get the grab position on the body
405  osg::Vec3 vGrabPos = VsMouseSpring::GetInstance()->GetGrabPosition() *osgRBBody->GetCameraMatrixTransform()->getWorldMatrices().at(0);
406  VsMouseSpring::GetInstance()->SetStart(vGrabPos);
407 
408  //get the 3D mouse coords
409  osg::Vec3 v3End = ConvertMouseTo3D(ea, x, y, vGrabPos);
410  VsMouseSpring::GetInstance()->SetEnd(v3End);
411 
412  //Calculate the force from the spring
413  osg::Vec3 vSpringLength = ((v3End - vGrabPos));
414  osg::Vec3 vSpringForce = vSpringLength * m_lpSim->MouseSpringStiffness();
415 
416  //sprintf(strDest, "Spring Force: (%3.3f, %3.3f, %3.3f)\n", vSpringForce[0], vSpringForce[1], vSpringForce[2]);
417  //OutputDebugString(strDest);
418 
419  //And the force from damping of the spring.
420  CStdFPoint fpBodyVel = rbBody->GetVelocityAtPoint(vGrabPos.x(), vGrabPos.y(), vGrabPos.z());
421  osg::Vec3 v3BodyVel(fpBodyVel.x, fpBodyVel.y, fpBodyVel.z);
422  osg::Vec3 v3DampForce = (v3BodyVel) * m_lpSim->MouseSpringDamping();
423 
424  //sprintf(strDest, "Damping Force: (%3.3f, %3.3f, %3.3f)\n", v3DampForce[0], v3DampForce[1], v3DampForce[2]);
425  //OutputDebugString(strDest);
426 
427  //Then the total force = Spring force - damping force
428  osg::Vec3 vTotalForce = vSpringForce - v3DampForce;
429  rbBody->AddForceAtWorldPos(vGrabPos.x(), vGrabPos.y(), vGrabPos.z(), vTotalForce.x(), vTotalForce.y(), vTotalForce.z(), false);
430 
431  //sprintf(strDest, "Length: (%3.3f, %3.3f, %3.3f) MS: (%3.1f, %3.1f, %3.1f)-(%3.1f, %3.1f, %3.1f) FORCE: (%3.1f, %3.1f, %3.1f)\n\n", vSpringLength[0], vSpringLength[1], vSpringLength[2], vGrabPos[0], vGrabPos[1], vGrabPos[2], v3End[0], v3End[1], v3End[2], vTotalForce[0], vTotalForce[1], vTotalForce[2]);
432  //OutputDebugString(strDest);
433 
434  return true;
435 }
436 
437 osg::Vec3 VsCameraManipulator::ConvertMouseTo3D(const GUIEventAdapter& ea, int x, int y, osg::Vec3 vGrabPos)
438 {
439 
440  //get the camera of the view
441  const osg::Camera *camera = m_osgViewer->getCamera();
442 
443  //get the viewport of the camera
444  const osg::Viewport* viewport = camera->getViewport();
445 
446  if(!viewport)
447  viewport = m_osgViewport.get();
448 
449  //convert normalized mouse points to pixels
450  float mx = viewport->x() + (int)((float)viewport->width()*(ea.getXnormalized()*0.5f+0.5f));
451  float my = viewport->y() + (int)((float)viewport->height()*(ea.getYnormalized()*0.5f+0.5f));
452 
453  //compute the final window matrix
454  osg::Matrix matrix = camera->getViewMatrix() * camera->getProjectionMatrix() * viewport->computeWindowMatrix();
455 
456  //invert it so I can get world coords from the window matrix for the mouse
457  osg::Matrix inverseWM ;
458  inverseWM.invert(matrix);
459 
460  // x, y come from ea.getX() and ea.getY()
461  osg::Vec3 near_point = osg::Vec3(mx, my, 0.0) * inverseWM;
462  osg::Vec3 far_point = osg::Vec3(mx, my, 1.0) * inverseWM;
463  float mouseDistance = (near_point - vGrabPos).length();
464 
465  osg::Vec3 clickDir = far_point - near_point;
466  clickDir.normalize();
467 
468  osg::Vec3 desiredPosition = near_point + (clickDir * mouseDistance);
469 
470  //char strDest[150];
471  //sprintf(strDest, "XY: (%3.1f, %3.1f, %3.1f),(%3.1f, %3.1f, %3.1f)\n", vGrabPos[0], vGrabPos[1], vGrabPos[2], desiredPosition[0], desiredPosition[1], desiredPosition[2]);
472  //OutputDebugString(strDest);
473 
474 
475  return desiredPosition;
476 }
477 
478 void VsCameraManipulator::DoPan(const GUIEventAdapter& ea, float x, float y)
479 {
480  float dx = x - m_fltPrevX;
481  float dy = y - m_fltPrevY;
482 
483  unsigned int buttonMask = ea.getButtonMask();
484  if(buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON))
485  {
486 
487  osg::Vec3 v3Eye(dx, 0, dy);
488  m_v3Eye = m_v3Eye - v3Eye;
489  }
490 }
491 
492 void VsCameraManipulator::DoZoom(const GUIEventAdapter& ea, float x, float y)
493 {
494  float dy = y - m_fltPrevY;
495 
496  unsigned int buttonMask = ea.getButtonMask();
497  if(buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON))
498  {
499  osg::Vec3 v3Eye(0, 0, dy);
500  m_v3Eye = m_v3Eye + v3Eye;
501  }
502 }
503 
504 void VsCameraManipulator::DoRotate(const GUIEventAdapter& ea, float x, float y)
505 {
506  float dx = x - m_fltPrevX;
507  float dy = y - m_fltPrevY;
508 
509  unsigned int buttonMask = ea.getButtonMask();
510  if(buttonMask==(GUIEventAdapter::LEFT_MOUSE_BUTTON))
511  {
512  osg::Matrix rotation_matrix(m_quatRotation);
513 
514  osg::Vec3 uv = osg::Vec3(0.0f,1.0f,0.0f)*rotation_matrix;
515  osg::Vec3 sv = osg::Vec3(1.0f,0.0f,0.0f)*rotation_matrix;
516  osg::Vec3 lv = osg::Vec3(0.0f,0.0f,-1.0f)*rotation_matrix;
517 
518  osg::Vec3 p2 = sv * x + uv * y - lv * tb_project_to_sphere(0.8f, x, y);
519  osg::Vec3 p1 = sv * m_fltPrevX + uv * m_fltPrevY - lv * tb_project_to_sphere(0.8f, m_fltPrevX, m_fltPrevY);
520 
521  osg::Vec3 axis;
522  axis = p2^p1;
523  axis.normalize();
524 
525  float t = (p2 - p1).length() / (2.0 * 0.8f);
526 
527  if (t > 1.0) t = 1.0;
528  if (t < -1.0) t = -1.0;
529 
530  float angle = osg::inRadians(asin(t));
531 
532  osg::Quat new_rotate;
533  new_rotate.makeRotate(angle,axis);
534 
535  m_quatRotation = m_quatRotation*new_rotate;
536 
537  /*osg::Matrixd matRot;
538  matRot = osg::Matrix::rotate(-dx, osg::Vec3(0,1,0)) *
539  osg::Matrix::rotate(dy, osg::Vec3(1,0,0));
540 
541  osg::Quat q = matRot.getRotate();
542  m_quatRotation = m_quatRotation * q;*/
543  }
544 }
545 
546 float VsCameraManipulator::tb_project_to_sphere(float r, float x, float y)
547 {
548  float d, t, z;
549 
550  d = sqrt(x*x + y*y);
551  /* Inside sphere */
552  if (d < r * 0.70710678118654752440)
553  {
554  z = sqrt(r*r - d*d);
555  } /* On hyperbola */
556  else
557  {
558  t = r / 1.41421356237309504880;
559  z = t*t / d;
560  }
561  return z;
562 }
563 
564  }// end Visualization
565 }// end VortexAnimatSim
VortexAnimatSim::Environment::VsRigidBody
A common class for all rigid body data specific to vortex.
Definition: VsRigidBody.h:55
VortexAnimatSim::Environment::VsMovableItem::GetCameraMatrixTransform
virtual osg::MatrixTransform * GetCameraMatrixTransform()
Gets the matrix transform used by the camera for the mouse spring.
Definition: VsMovableItem.cpp:454
VortexAnimatSim
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
Definition: MeshMinVertexDistanceVisitor.cpp:5
VsLight.h
Declares the vortex Light class.
VortexAnimatSim::Visualization::VsCameraManipulator::handle
virtual bool handle(const GUIEventAdapter &ea, GUIActionAdapter &us)
Definition: VsCameraManipulator.cpp:46
StdUtils::Std_CalculateDistance
double Std_CalculateDistance(CStdIPoint &ptA, CStdIPoint &ptB)
Calculates the distance between two points.
Definition: StdSerialize.cpp:455
VsStructure.h
Declares the vortex structure class.