AnimatLab  2
Test
VsHinge.cpp
Go to the documentation of this file.
1 
7 #include "StdAfx.h"
8 #include "VsMovableItem.h"
9 #include "VsBody.h"
10 #include "VsJoint.h"
11 #include "VsMotorizedJoint.h"
12 #include "VsRigidBody.h"
13 #include "VsJoint.h"
14 #include "VsHingeLimit.h"
15 #include "VsHinge.h"
16 #include "VsStructure.h"
17 #include "VsSimulator.h"
18 #include "VsOsgUserData.h"
19 #include "VsOsgUserDataVisitor.h"
20 #include "VsDragger.h"
21 
22 namespace VortexAnimatSim
23 {
24  namespace Environment
25  {
26  namespace Joints
27  {
28 
36 {
37  SetThisPointers();
38  m_vxHinge = NULL;
39 
42  m_lpPosFlap = new VsHingeLimit();
43 
44  m_lpUpperLimit->LimitPos(0.25*VX_PI, false);
45  m_lpLowerLimit->LimitPos(-0.25*VX_PI, false);
46  m_lpPosFlap->LimitPos(Hinge::JointPosition(), false);
48 
49  m_lpUpperLimit->Color(1, 0, 0, 1);
50  m_lpLowerLimit->Color(1, 1, 1, 1);
51  m_lpPosFlap->Color(0, 0, 1, 1);
52 
55 }
56 
64 {
65  //ConstraintLimits are deleted in the base objects.
66  try
67  {
68  DeleteGraphics();
69  DeletePhysics();
70  }
71  catch(...)
72  {Std_TraceMsg(0, "Caught Error in desctructor of VsHinge\r\n", "", -1, false, true);}
73 }
74 
75 void VsHinge::EnableLimits(bool bVal)
76 {
77  Hinge::EnableLimits(bVal);
78 
79  if(m_vxHinge)
80  m_vxHinge->setLimitsActive(m_vxHinge->kAngularCoordinate, m_bEnableLimits);
81 
82  if(m_bEnableLimits)
83  {
86  }
87 }
88 
89 void VsHinge::JointPosition(float fltPos)
90 {
91  m_fltPosition = fltPos;
92  if(m_lpPosFlap)
93  m_lpPosFlap->LimitPos(fltPos);
94 }
95 
96 void VsHinge::SetAlpha()
97 {
98  VsJoint::SetAlpha();
99 
103 
104  if(m_osgCylinderMat.valid() && m_osgCylinderSS.valid())
105  SetMaterialAlpha(m_osgCylinderMat.get(), m_osgCylinderSS.get(), m_fltAlpha);
106 
107 }
108 
109 void VsHinge::DeleteJointGraphics()
110 {
111  VsHingeLimit *lpUpperLimit = dynamic_cast<VsHingeLimit *>(m_lpUpperLimit);
112  VsHingeLimit *lpLowerLimit = dynamic_cast<VsHingeLimit *>(m_lpLowerLimit);
113  VsHingeLimit *lpPosFlap = dynamic_cast<VsHingeLimit *>(m_lpPosFlap);
114 
115  if(m_osgJointMT.valid())
116  {
117  if(m_osgCylinderMT.valid()) m_osgJointMT->removeChild(m_osgCylinderMT.get());
118  if(lpUpperLimit && lpUpperLimit->FlapTranslateMT()) m_osgJointMT->removeChild(lpUpperLimit->FlapTranslateMT());
119  if(lpLowerLimit && lpLowerLimit->FlapTranslateMT()) m_osgJointMT->removeChild(lpLowerLimit->FlapTranslateMT());
120  if(lpPosFlap && lpPosFlap->FlapTranslateMT()) m_osgJointMT->removeChild(lpPosFlap->FlapTranslateMT());
121  }
122 
123  m_osgCylinder.release();
124  m_osgCylinderGeode.release();
125  m_osgCylinderMT.release();
126  m_osgCylinderMat.release();
127  m_osgCylinderSS.release();
128 
132 }
133 
141 {
142  //Create the cylinder for the hinge
144  m_osgCylinderGeode = new osg::Geode;
145  m_osgCylinderGeode->addDrawable(m_osgCylinder.get());
146 
147  CStdFPoint vPos(0, 0, 0), vRot(VX_PI/2, 0, 0);
148  m_osgCylinderMT = new osg::MatrixTransform();
149  m_osgCylinderMT->setMatrix(SetupMatrix(vPos, vRot));
150  m_osgCylinderMT->addChild(m_osgCylinderGeode.get());
151 
152  //create a material to use with the pos flap
153  if(!m_osgCylinderMat.valid())
154  m_osgCylinderMat = new osg::Material();
155 
156  //create a stateset for this node
157  m_osgCylinderSS = m_osgCylinderMT->getOrCreateStateSet();
158 
159  //set the diffuse property of this node to the color of this body
160  m_osgCylinderMat->setAmbient(osg::Material::FRONT_AND_BACK, osg::Vec4(0.1, 0.1, 0.1, 1));
161  m_osgCylinderMat->setDiffuse(osg::Material::FRONT_AND_BACK, osg::Vec4(1, 0.25, 1, 1));
162  m_osgCylinderMat->setSpecular(osg::Material::FRONT_AND_BACK, osg::Vec4(0.25, 0.25, 0.25, 1));
163  m_osgCylinderMat->setShininess(osg::Material::FRONT_AND_BACK, 64);
164  m_osgCylinderSS->setMode(GL_BLEND, osg::StateAttribute::OVERRIDE | osg::StateAttribute::ON);
165 
166  //apply the material
167  m_osgCylinderSS->setAttribute(m_osgCylinderMat.get(), osg::StateAttribute::ON);
168 }
169 
171 {
173 
174  VsHingeLimit *lpUpperLimit = dynamic_cast<VsHingeLimit *>(m_lpUpperLimit);
175  VsHingeLimit *lpLowerLimit = dynamic_cast<VsHingeLimit *>(m_lpLowerLimit);
176  VsHingeLimit *lpPosFlap = dynamic_cast<VsHingeLimit *>(m_lpPosFlap);
177 
178  lpPosFlap->LimitPos(Hinge::JointPosition());
179 
180  lpUpperLimit->SetupGraphics();
181  lpLowerLimit->SetupGraphics();
182  lpPosFlap->SetupGraphics();
183 
184  m_osgJointMT->addChild(m_osgCylinderMT.get());
185  m_osgJointMT->addChild(lpUpperLimit->FlapTranslateMT());
186  m_osgJointMT->addChild(lpLowerLimit->FlapTranslateMT());
187  m_osgJointMT->addChild(lpPosFlap->FlapTranslateMT());
188 }
189 
191 {
192  //The parent osg object for the joint is actually the child rigid body object.
193  m_osgParent = ParentOSG();
194 
195  if(m_osgParent.valid())
196  {
197  //Add the parts to the group node.
198  CStdFPoint vPos(0, 0, 0), vRot(VX_PI/2, 0, 0);
199  vPos.Set(0, 0, 0); vRot.Set(0, VX_PI/2, 0);
200 
201  m_osgJointMT = new osg::MatrixTransform();
202  m_osgJointMT->setMatrix(SetupMatrix(vPos, vRot));
203 
205 
206  m_osgNode = m_osgJointMT.get();
207 
208  BuildLocalMatrix();
209 
210  SetAlpha();
211  SetCulling();
212  SetVisible(m_lpThisMI->IsVisible());
213 
214  //Add it to the scene graph.
215  m_osgParent->addChild(m_osgRoot.get());
216 
217  //Set the position with the world coordinates.
218  Physics_UpdateAbsolutePosition();
219 
220  //We need to set the UserData on the OSG side so we can do picking.
221  //We need to use a node visitor to set the user data for all drawable nodes in all geodes for the group.
222  osg::ref_ptr<VsOsgUserDataVisitor> osgVisitor = new VsOsgUserDataVisitor(this);
223  osgVisitor->traverse(*m_osgMT);
224  }
225 }
226 
227 void VsHinge::DeletePhysics()
228 {
229  if(!m_vxHinge)
230  return;
231 
232  if(GetVsSimulator() && GetVsSimulator()->Universe())
233  {
234  GetVsSimulator()->Universe()->removeConstraint(m_vxHinge);
235  delete m_vxHinge;
236 
237  if(m_lpChild && m_lpParent)
239  }
240 
241  m_vxHinge = NULL;
242  m_vxJoint = NULL;
243 }
244 
245 void VsHinge::SetupPhysics()
246 {
247  if(m_vxHinge)
248  DeletePhysics();
249 
250  if(!m_lpParent)
251  THROW_ERROR(Al_Err_lParentNotDefined, Al_Err_strParentNotDefined);
252 
253  if(!m_lpChild)
254  THROW_ERROR(Al_Err_lChildNotDefined, Al_Err_strChildNotDefined);
255 
256  VsRigidBody *lpVsParent = dynamic_cast<VsRigidBody *>(m_lpParent);
257  if(!lpVsParent)
258  THROW_ERROR(Vs_Err_lUnableToConvertToVsRigidBody, Vs_Err_strUnableToConvertToVsRigidBody);
259 
260  VsRigidBody *lpVsChild = dynamic_cast<VsRigidBody *>(m_lpChild);
261  if(!lpVsChild)
262  THROW_ERROR(Vs_Err_lUnableToConvertToVsRigidBody, Vs_Err_strUnableToConvertToVsRigidBody);
263 
264  CStdFPoint vGlobal = this->GetOSGWorldCoords();
265 
266  Vx::VxReal44 vMT;
267  VxOSG::copyOsgMatrix_to_VxReal44(this->GetOSGWorldMatrix(), vMT);
268  Vx::VxTransform vTrans(vMT);
269  Vx::VxReal3 vxRot;
270  vTrans.getRotationEulerAngles(vxRot);
271 
272  CStdFPoint vLocalRot(vxRot[0], vxRot[1], vxRot[2]); //= m_lpThisMI->Rotation();
273 
274  VxVector3 pos((double) vGlobal.x, (double) vGlobal.y, (double) vGlobal.z);
275  VxVector3 axis = NormalizeAxis(vLocalRot);
276 
277  m_vxHinge = new VxHinge(lpVsParent->Part(), lpVsChild->Part(), pos.v, axis.v);
278  m_vxHinge->setName(m_strID.c_str());
279 
280  GetVsSimulator()->Universe()->addConstraint(m_vxHinge);
281 
282  //Disable collisions between this object and its parent
284 
285  VsHingeLimit *lpUpperLimit = dynamic_cast<VsHingeLimit *>(m_lpUpperLimit);
286  VsHingeLimit *lpLowerLimit = dynamic_cast<VsHingeLimit *>(m_lpLowerLimit);
287 
288  lpUpperLimit->HingeRef(m_vxHinge);
289  lpLowerLimit->HingeRef(m_vxHinge);
290 
291  //Re-enable the limits once we have initialized the joint
293 
294  m_vxJoint = m_vxHinge;
295  m_iCoordID = m_vxHinge->kAngularCoordinate;
296 
297  //If the motor is enabled then it will start out with a velocity of zero.
299 
300  Hinge::Initialize();
301  VsJoint::Initialize();
302 }
303 
305 {
306  SetupGraphics();
307  SetupPhysics();
308 }
309 
310 #pragma region DataAccesMethods
311 
312 float *VsHinge::GetDataPointer(const std::string &strDataType)
313 {
314  float *lpData=NULL;
315  std::string strType = Std_CheckString(strDataType);
316 
317  if(strType == "JOINTROTATION")
318  return &m_fltPosition;
319  else if(strType == "JOINTPOSITION")
320  return &m_fltPosition;
321  else if(strType == "JOINTACTUALVELOCITY")
322  return &m_fltVelocity;
323  else if(strType == "JOINTFORCE")
324  return &m_fltForce;
325  else if(strType == "JOINTROTATIONDEG")
326  return &m_fltRotationDeg;
327  else if(strType == "JOINTDESIREDPOSITIONDEG")
328  return &m_fltDesiredPositionDeg;
329  else if(strType == "JOINTDESIREDVELOCITY")
330  return &m_fltReportSetVelocity;
331  else if(strType == "JOINTSETVELOCITY")
332  return &m_fltReportSetVelocity;
333  else if(strType == "JOINTDESIREDPOSITION")
334  return &m_fltReportSetPosition;
335  else if(strType == "JOINTSETPOSITION")
336  return &m_fltReportSetPosition;
337  else if(strType == "ENABLE")
338  return &m_fltEnabled;
339  else if(strType == "MOTORFORCETOAX")
340  return &m_fltNullReport;
341  else if(strType == "MOTORFORCETOAY")
342  return &m_fltNullReport;
343  else if(strType == "MOTORFORCETOAZ")
344  return &m_fltNullReport;
345  else if(strType == "MOTORFORCETOBX")
346  return &m_fltNullReport;
347  else if(strType == "MOTORFORCETOBY")
348  return &m_fltNullReport;
349  else if(strType == "MOTORFORCETOBZ")
350  return &m_fltNullReport;
351  else if(strType == "MOTORTORQUETOAX")
352  return &m_fltNullReport;
353  else if(strType == "MOTORTORQUETOAY")
354  return &m_fltNullReport;
355  else if(strType == "MOTORTORQUETOAZ")
356  return &m_fltNullReport;
357  else if(strType == "MOTORTORQUETOBX")
358  return &m_fltNullReport;
359  else if(strType == "MOTORTORQUETOBY")
360  return &m_fltNullReport;
361  else if(strType == "MOTORTORQUETOBZ")
362  return &m_fltNullReport;
363  else if(strType == "CONTACTCOUNT")
364  THROW_PARAM_ERROR(Al_Err_lMustBeContactBodyToGetCount, Al_Err_strMustBeContactBodyToGetCount, "JointID", m_strName);
365  else
366  {
367  lpData = Hinge::GetDataPointer(strDataType);
368  if(lpData) return lpData;
369 
370  THROW_TEXT_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "JointID: " + STR(m_strName) + " DataType: " + strDataType);
371  }
372 
373  return lpData;
374 }
375 
376 bool VsHinge::SetData(const std::string &strDataType, const std::string &strValue, bool bThrowError)
377 {
378  if(VsJoint::Physics_SetData(strDataType, strValue))
379  return true;
380 
381  if(Hinge::SetData(strDataType, strValue, false))
382  return true;
383 
384  //If it was not one of those above then we have a problem.
385  if(bThrowError)
386  THROW_PARAM_ERROR(Al_Err_lInvalidDataType, Al_Err_strInvalidDataType, "Data Type", strDataType);
387 
388  return false;
389 }
390 
391 void VsHinge::QueryProperties(CStdPtrArray<TypeProperty> &aryProperties)
392 {
393  VsJoint::Physics_QueryProperties(aryProperties);
394  Hinge::QueryProperties(aryProperties);
395 
396  aryProperties.Add(new TypeProperty("JointRotation", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
397  aryProperties.Add(new TypeProperty("JointPosition", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
398  aryProperties.Add(new TypeProperty("JointActualVelocity", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
399  aryProperties.Add(new TypeProperty("JointForce", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
400  aryProperties.Add(new TypeProperty("JointRotationDeg", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
401  aryProperties.Add(new TypeProperty("JointDesiredVelocity", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
402  aryProperties.Add(new TypeProperty("JointSetVelocity", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
403  aryProperties.Add(new TypeProperty("JointDesiredPosition", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
404  aryProperties.Add(new TypeProperty("JointDesiredPositionDeg", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
405  aryProperties.Add(new TypeProperty("JointSetPosition", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
406  aryProperties.Add(new TypeProperty("Enable", AnimatPropertyType::Boolean, AnimatPropertyDirection::Get));
407  aryProperties.Add(new TypeProperty("MotorForceToAX", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
408  aryProperties.Add(new TypeProperty("MotorForceToAY", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
409  aryProperties.Add(new TypeProperty("MotorForceToAZ", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
410  aryProperties.Add(new TypeProperty("MotorForceToBX", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
411  aryProperties.Add(new TypeProperty("MotorForceToBY", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
412  aryProperties.Add(new TypeProperty("MotorForceToBZ", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
413  aryProperties.Add(new TypeProperty("MotorTorqueToAX", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
414  aryProperties.Add(new TypeProperty("MotorTorqueToAY", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
415  aryProperties.Add(new TypeProperty("MotorTorqueToAZ", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
416  aryProperties.Add(new TypeProperty("MotorTorqueToBX", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
417  aryProperties.Add(new TypeProperty("MotorTorqueToBY", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
418  aryProperties.Add(new TypeProperty("MotorTorqueToBZ", AnimatPropertyType::Float, AnimatPropertyDirection::Get));
419 }
420 
421 #pragma endregion
422 
424 {
425  UpdateData();
427 }
428 
429  } //Joints
430  } // Environment
431 } //VortexAnimatSim
float CylinderHeight()
Gets the height of the cylinder used to display the hinge in the environment.
Definition: Hinge.cpp:114
virtual void DisableCollision(RigidBody *lpBody)
Disables collision between the past-in object and this object.
Definition: RigidBody.cpp:1374
virtual void EnableCollision(RigidBody *lpBody)
Enables collision between the past-in object and this object.
Definition: RigidBody.cpp:1343
virtual float LimitPos()
Gets the limit position.
virtual void SetupGraphics()
Sets up the graphics for the joint.
Definition: VsHinge.cpp:190
virtual void CreateJoint()
Creates the joint.
Definition: VsHinge.cpp:304
float m_fltNullReport
This is used to report back 0 from GetDataPointer for items that are not supported in vortex...
Definition: VsMovableItem.h:40
A common class for all rigid body data specific to vortex.
Definition: VsRigidBody.h:55
virtual void Alpha(float fltA)=0
Sets the alpha color value for this constraint.
osg::ref_ptr< osg::MatrixTransform > m_osgCylinderMT
The osg cylinder matrix transform.
Definition: VsHinge.h:50
float m_fltEnabled
This is used for reporting the enabled state in a GetDataPointer call.
Definition: Node.h:35
osg::ref_ptr< osg::Material > m_osgCylinderMat
The osg cylinder material.
Definition: VsHinge.h:53
ConstraintLimit * m_lpUpperLimit
Upper limit constring pointer.
Definition: Hinge.h:43
virtual void CreateJointGraphics()
Creates the default ball graphics.
Definition: VsHinge.cpp:170
virtual void IsShowPosition(bool bVal)
Sets whether this contstraint is actually just being used to show the current position of the joint...
osg::Geometry * CreateConeGeometry(float height, float topradius, float botradius, int sides, bool doSide, bool doTop, bool doBottom)
virtual void StepSimulation()
Step the simulation for this object.
Definition: VsHinge.cpp:423
std::string m_strID
The unique Id for this object.
Definition: AnimatBase.h:55
float m_fltRotationDeg
The rotation of the hinge in degrees.
Definition: Hinge.h:52
float CylinderRadius()
Gets the radius cylinder of the cylinder used to display the hinge in the environment.
Definition: Hinge.cpp:101
float m_fltDesiredPositionDeg
The desired rotation of the hinge in degrees.
Definition: Hinge.h:55
virtual void SetupGraphics()
Sets up the graphics for the constraint.
osg::ref_ptr< osg::Geode > m_osgCylinderGeode
The osg cylinder geode.
Definition: VsHinge.h:47
virtual bool EnableMotor()
Tells if the motor is enabled.
RigidBody * m_lpChild
The child rigid body for this joint.
Definition: Joint.h:44
bool m_bEnableLimits
If true then any ConstraintLimits for this joint are enabled.
Definition: Joint.h:47
Vx::VxHinge * m_vxHinge
The vortex hinge class.
Definition: VsHinge.h:40
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
ConstraintLimit * m_lpLowerLimit
Lower limit constring pointer.
Definition: Hinge.h:46
Declares the vortex hinge class.
virtual void CreateCylinderGraphics()
Creates the cylinder graphics.
Definition: VsHinge.cpp:140
void Std_TraceMsg(const int iLevel, std::string strMessage, std::string strSourceFile, int iSourceLine, bool bLogToFile, bool bPrintHeader)
Traces a message to the debugger window.
float m_fltForce
The current force being applied to the joint by the motor.
Definition: Joint.h:58
osg::ref_ptr< osg::Geometry > m_osgCylinder
The osg cylinder geometry.
Definition: VsHinge.h:44
osg::ref_ptr< osg::MatrixTransform > m_osgJointMT
The osg joint matrix transform.
Definition: VsJoint.h:61
osg::ref_ptr< osg::StateSet > m_osgCylinderSS
The osg cylinder state set.
Definition: VsHinge.h:56
float m_fltReportSetPosition
This is the variable that is reported to AnimatLab on what the set position was.
virtual void SetLimitPos()=0
Sets the limit position using the current value set within the object.
ConstraintLimit * m_lpPosFlap
Pointer to a constraint that is used to represent the position flap.
Definition: Hinge.h:49
float m_fltAlpha
The current alpha transparency for this body part.
Definition: MovableItem.h:76
float m_fltReportSetVelocity
This is the variable that is reported to AnimatLab on what the set veloicty was.
virtual void SetVelocityToDesired()
Sets the desired velocity to use for the motor.
virtual float JointPosition()
Gets the joint position.
Definition: Joint.cpp:532
virtual void DeleteGraphics()=0
Deletes up the graphics for the constraint.
virtual void Color(float fltR, float fltG, float fltB, float fltA)
Sets the color to use when displaying this contraint.
virtual void IsLowerLimit(bool bVal)
Sets whether this is a lower limit or not..
std::string Std_CheckString(std::string strVal)
Converts a string to upper case and trims it.
virtual void UpdateData()
Called to collect any body data for this part.
Definition: Hinge.cpp:219
virtual bool EnableLimits()
Tells if ConstraintLimits are enabled.
Definition: Joint.cpp:139
std::string m_strName
The name for this object.
Definition: AnimatBase.h:61
Declares the vortex structure class.