AnimatLab  2
Test
VsLine.cpp
1 // VsLine.cpp: implementation of the VsLine class.
2 //
4 
5 #include "StdAfx.h"
6 #include "VsMovableItem.h"
7 #include "VsBody.h"
8 #include "VsJoint.h"
9 #include "VsMotorizedJoint.h"
10 #include "VsRigidBody.h"
11 #include "VsLine.h"
12 #include "VsBox.h"
13 #include "VsSimulator.h"
14 #include "VsDragger.h"
15 
16 
17 namespace VortexAnimatSim
18 {
19  namespace Environment
20  {
21 
23 // Construction/Destruction
25 
26 VsLine::VsLine()
27 {
28 }
29 
30 VsLine::~VsLine()
31 {
32 
33 try
34 {
35 }
36 catch(...)
37 {Std_TraceMsg(0, "Caught Error in desctructor of VsLine\r\n", "", -1, false, true);}
38 }
39 
40 void VsLine::SetThisPointers()
41 {
42  VsRigidBody::SetThisPointers();
43 
44  m_lpLineBase = dynamic_cast<LineBase *>(this);
45  if(!m_lpLineBase)
46  THROW_TEXT_ERROR(Vs_Err_lThisPointerNotDefined, Vs_Err_strThisPointerNotDefined, "m_lpLineBase, " + m_lpThisAB->Name());
47 }
48 
49 osg::Geometry *VsLine::CreateLineGeometry()
50 {
51  osg::Geometry* linesGeom = new osg::Geometry();
52  int iCount = BuildLines(linesGeom);
53 
54  // set the colors as before, plus using the above
55  CStdColor &aryDiffuse = *m_lpThisRB->Diffuse();
56  osg::Vec4Array* colors = new osg::Vec4Array;
57  colors->push_back(osg::Vec4(aryDiffuse[0], aryDiffuse[1], aryDiffuse[2], aryDiffuse[3]));
58  linesGeom->setColorArray(colors);
59  linesGeom->setColorBinding(osg::Geometry::BIND_OVERALL);
60 
61 
62  // set the normal in the same way color.
63  osg::Vec3Array* normals = new osg::Vec3Array;
64  normals->push_back(osg::Vec3(0.0f,-1.0f,0.0f));
65  linesGeom->setNormalArray(normals);
66  linesGeom->setNormalBinding(osg::Geometry::BIND_OVERALL);
67 
68 
69  // This time we simply use primitive, and hardwire the number of coords to use
70  // since we know up front,
71  osg::PrimitiveSet *lpSet = new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, iCount);
72  linesGeom->addPrimitiveSet(lpSet);
73 
74  linesGeom->setDataVariance(osg::Object::DYNAMIC);
75  linesGeom->setUseDisplayList(false);
76 
77  return linesGeom;
78 }
79 
80 int VsLine::BuildLines(osg::Geometry *linesGeom)
81 {
82  //Always create the graphics. If it is disabled or made not visible we will turn it off.
83  CStdArray<Attachment *> *aryAttachments = m_lpLineBase->AttachmentPoints();
84  int iCount = aryAttachments->GetSize();
85 
86  //If we already have a lines array then delete it and rebuild.
87  if(m_aryLines.valid())
88  m_aryLines.release();
89 
90  m_aryLines = new osg::Vec3Array;
91 
92  //Only draw the line if we have more than one point.
93  if(iCount > 1)
94  {
95  CStdFPoint vPos;
96  Attachment *lpPrevAttach=NULL;
97  Attachment *lpAttach=NULL;
98 
99  for(int iIndex=1; iIndex<iCount; iIndex++)
100  {
101  lpPrevAttach = aryAttachments->at(iIndex-1);
102  lpAttach = aryAttachments->at(iIndex);
103 
104  vPos = lpPrevAttach->AbsolutePosition();
105  m_aryLines->push_back(osg::Vec3(vPos.x, vPos.y, vPos.z));
106 
107  vPos = lpAttach->AbsolutePosition();
108  m_aryLines->push_back(osg::Vec3(vPos.x, vPos.y, vPos.z));
109  }
110  }
111 
112  linesGeom->setVertexArray(m_aryLines.get());
113  linesGeom->dirtyBound();
114 
115  return m_aryLines->getNumElements();
116 }
117 
118 void VsLine::DrawLine()
119 {
120  CStdArray<Attachment *> *aryAttachments = m_lpLineBase->AttachmentPoints();
121  int iCount = aryAttachments->GetSize();
122 
123  //Only draw the line if we have more than one point.
124  if(iCount > 1)
125  {
126  CStdFPoint vPos;
127  Attachment *lpPrevAttach=NULL;
128  Attachment *lpAttach=NULL;
129  int iLineIdx = 0;
130 
131  for(int iIndex=1; iIndex<iCount; iIndex++)
132  {
133  lpPrevAttach = aryAttachments->at(iIndex-1);
134  lpAttach = aryAttachments->at(iIndex);
135 
136  vPos = lpPrevAttach->AbsolutePosition();
137  (*m_aryLines)[iLineIdx].set(vPos.x, vPos.y, vPos.z);
138 
139  vPos = lpAttach->AbsolutePosition();
140  (*m_aryLines)[iLineIdx+1].set(vPos.x, vPos.y, vPos.z);
141 
142  iLineIdx+=2;
143  }
144 
145  m_osgGeometry->dirtyBound();
146  }
147 }
148 
149 void VsLine::SetupGraphics()
150 {
151  //Add it to the root scene graph because the vertices are in global coords.
152  GetVsSimulator()->OSGRoot()->addChild(m_osgNode.get());
153  SetVisible(m_lpThisMI->IsVisible());
154 }
155 
156 void VsLine::DeleteGraphics()
157 {
158  if(m_osgGeometry.valid())
159  {
160  m_osgGeometry->setDataVariance(osg::Object::STATIC);
161  m_osgGeometry->dirtyBound();
162  SetVisible(false);
163  }
164 
165  VsRigidBody::DeleteGraphics();
166 }
167 
168 void VsLine::CreateGraphicsGeometry()
169 {
170  fltA = 0;
171  m_osgGeometry = CreateLineGeometry();
172 }
173 
174 void VsLine::CreatePhysicsGeometry()
175 {
176  m_vxGeometry = NULL;
177 }
178 
179 void VsLine::CreateParts()
180 {
181  CreateGeometry();
182 
183  VsRigidBody::CreateItem();
184  VsRigidBody::SetBody();
185 }
186 
187 void VsLine::CalculateForceVector(Attachment *lpPrim, Attachment *lpSec, float fltTension, CStdFPoint &oPrimPos, CStdFPoint &oSecPos, CStdFPoint &oPrimForce)
188 {
189  oPrimPos = lpPrim->AbsolutePosition();
190  oSecPos = lpSec->AbsolutePosition();
191 
192  oPrimForce = oSecPos - oPrimPos;
193  oPrimForce.Normalize();
194  oPrimForce *= fltTension;
195 }
196 
197 void VsLine::StepSimulation(float fltTension)
198 {
199  if(m_lpThisBP->Enabled())
200  {
201  //Dont bother with this unless there is actually tension developed by the muscle.
202  if(fltTension > 1e-5)
203  {
204  CStdArray<Attachment *> *aryAttachments = m_lpLineBase->AttachmentPoints();
205  int iCount = aryAttachments->GetSize();
206  Attachment *lpAttach1 = aryAttachments->at(0), *lpAttach2 = NULL;
207  CStdFPoint oPrimPos, oPrimPlusPos, oSecPos, oSecMinusPos;
208  CStdFPoint oPrimForce, oSecForce;
209  RigidBody *lpAttach1Parent, *lpAttach2Parent;
210 
211  //if(m_lpThisBP->GetSimulator()->Time() >= 0.99)
212  // iCount = iCount;
213 
214  //Go through each set of muscle attachments and add the tension force pointing towards the other
215  //attachment point at each connector.
216  for(int iIndex=1; iIndex<iCount; iIndex++)
217  {
218  lpAttach2 = aryAttachments->at(iIndex);
219 
220  lpAttach1Parent = lpAttach1->Parent();
221  lpAttach2Parent = lpAttach2->Parent();
222 
223  CalculateForceVector(lpAttach1, lpAttach2, fltTension, oPrimPos, oPrimPlusPos, oPrimForce);
224  CalculateForceVector(lpAttach2, lpAttach1, fltTension, oSecPos, oSecMinusPos, oSecForce);
225 
226  lpAttach1Parent->AddForceAtWorldPos(oPrimPos.x, oPrimPos.y, oPrimPos.z, oPrimForce.x, oPrimForce.y, oPrimForce.z, true);
227  lpAttach2Parent->AddForceAtWorldPos(oSecPos.x, oSecPos.y, oSecPos.z, oSecForce.x, oSecForce.y, oSecForce.z, true);
228 
229  lpAttach1 = lpAttach2;
230  }
231  }
232  }
233 
234  DrawLine();
235 }
236 
237 void VsLine::ResetSimulation()
238 {
239  //We do nothing in the reset simulation because we need the attachment points to be reset before we can do anything.
240 }
241 
242 void VsLine::AfterResetSimulation()
243 {
244  DrawLine();
245 }
246 
247 
248  } // Visualization
249 } //VortexAnimatSim
Classes for implementing the cm-labs vortex physics engine for AnimatLab.
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.