CGELSkeletonLink.h
Go to the documentation of this file.
1 //===========================================================================
2 /*
3  Software License Agreement (BSD License)
4  Copyright (c) 2003-2016, CHAI3D.
5  (www.chai3d.org)
6 
7  All rights reserved.
8 
9  Redistribution and use in source and binary forms, with or without
10  modification, are permitted provided that the following conditions
11  are met:
12 
13  * Redistributions of source code must retain the above copyright
14  notice, this list of conditions and the following disclaimer.
15 
16  * Redistributions in binary form must reproduce the above
17  copyright notice, this list of conditions and the following
18  disclaimer in the documentation and/or other materials provided
19  with the distribution.
20 
21  * Neither the name of CHAI3D nor the names of its contributors may
22  be used to endorse or promote products derived from this software
23  without specific prior written permission.
24 
25  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
26  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
27  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
28  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
29  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
30  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
31  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
32  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
33  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
34  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
35  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
36  POSSIBILITY OF SUCH DAMAGE.
37 
38  \author <http://www.chai3d.org>
39  \author Francois Conti
40  \version 3.2.0 $Rev: 1869 $
41 */
42 //===========================================================================
43 
44 //---------------------------------------------------------------------------
45 #ifndef CGELSkeletonLinkH
46 #define CGELSkeletonLinkH
47 //---------------------------------------------------------------------------
48 #include "chai3d.h"
49 #include "CGELSkeletonNode.h"
50 //---------------------------------------------------------------------------
51 
52 //===========================================================================
60 //===========================================================================
61 
62 //===========================================================================
70 //===========================================================================
72 {
73  //-----------------------------------------------------------------------
74  // CONSTRUCTOR & DESTRUCTOR:
75  //-----------------------------------------------------------------------
76 
77 public:
78 
81 
84 
85 
86  //-----------------------------------------------------------------------
87  // METHODS:
88  //-----------------------------------------------------------------------
89 
90 public:
91 
92  //-----------------------------------------------------------------------
97  //-----------------------------------------------------------------------
98  inline void render()
99  {
100  // render link
101  glColor4fv( (const float *)&m_color);
102  glBegin(GL_LINES);
103  glVertex3dv( (const double *)&m_node0->m_pos);
104  glVertex3dv( (const double *)&m_node1->m_pos);
105  glEnd();
106  }
107 
108 
109  //-----------------------------------------------------------------------
115  //-----------------------------------------------------------------------
116  inline void computeForces()
117  {
118  // update basic parameters of current link
120  m_wLink10 = cMul(-1, m_wLink01);
122 
125 
126  //-------------------------------------------------------------
127  // ELONGATION:
128  //-------------------------------------------------------------
129  // if distance too small, no forces are applied
130  if (m_length < 0.000001) { return; }
131 
132  // elongation compute force
133  double f = m_kSpringElongation * (m_length - m_length0);
134 
135  // apply force
136  if (m_length > 0.000001)
137  {
139  m_node0->addForce(force);
140  chai3d::cVector3d tmpfrc = cMul(-1, force);
141  m_node1->addForce(tmpfrc);
142  }
143 
144  //-------------------------------------------------------------
145  // FLEXION:
146  //-------------------------------------------------------------
147  // compute flexion forces and torques
150  double angle0 = cAngle(m_wzLink01, m_wLink01);
151  double angle1 = cAngle(m_wzLink10, m_wLink10);
152  double torqueMag0 = angle0 * m_kSpringFlexion;
153  double torqueMag1 = angle1 * m_kSpringFlexion;
154 
155  if ((m_length > 0.000001) && (m_enabled))
156  {
157  if (torqueMag0 > 0.0001)
158  {
159  chai3d::cVector3d torque0 = cMul(torqueMag0, cNormalize(torqueDir0));
160  m_node0->addTorque(torque0);
161  chai3d::cVector3d force1 = cMul((torqueMag0/m_length), cNormalize(cCross(m_wLink01, torque0)));
162  m_node1->addForce(force1);
163  chai3d::cVector3d tmpfrc = cMul(-1,force1);
164  m_node0->addForce(tmpfrc);
165  }
166 
167  if (torqueMag1 > 0.0001)
168  {
169  chai3d::cVector3d torque1 = cMul(torqueMag1, cNormalize(torqueDir1));
170  m_node1->addTorque(torque1);
171  chai3d::cVector3d force0 = cMul((torqueMag1/m_length), cNormalize(cCross(m_wLink10, torque1)));
172  m_node0->addForce(force0);
173  chai3d::cVector3d tmpfrc = cMul(-1,force0);
174  m_node1->addForce(tmpfrc);
175  }
176  }
177 
178  //-----------------------------------------------------------------------
179  // TORSION:
180  //-----------------------------------------------------------------------
181 
182  if (m_enabled)
183  {
184  // update frame vectors in world coordinates.
188 
189  // compute torsional angle and torque
190  chai3d::cVector3d v0, v1;
191  m_wA0.crossr(m_wLink01, v0);
192  m_wA1.crossr(m_wLink01, v1);
193  v0.normalize();
194  v1.normalize();
195 
196  chai3d::cVector3d torque;
197  v0.crossr(v1, m_torsionAxisAngle);
199 
200  m_node0->addTorque(torque);
201  chai3d::cVector3d tmptrq = -1.0 * torque;
202  m_node1->addTorque(tmptrq);
203  }
204  }
205 
206 
207  //-----------------------------------------------------------------------
208  // PUBLIC MEMBERS - MASS NODES:
209  //-----------------------------------------------------------------------
210 
211 public:
212 
215 
218 
219 
220  //-----------------------------------------------------------------------
221  // PUBLIC MEMBERS - GRAPHICAL PROPERTIES:
222  //-----------------------------------------------------------------------
223 
224 public:
225 
228 
229 
230  //-----------------------------------------------------------------------
231  // PUBLIC MEMBERS - PHYSICAL PROPERTIES:
232  //-----------------------------------------------------------------------
233 
234 public:
235 
238 
241 
244 
246  bool m_enabled;
247 
249  //double m_kDamperElongation;
250 
252  //double m_kDamperFlexion;
253 
255  //double m_kDamperTorsion;
256 
257 
258  //-----------------------------------------------------------------------
259  // PUBLIC MEMBERS:
260  //-----------------------------------------------------------------------
261 
262 public:
263 
266 
269 
272 
275 
278 
281 
284 
287 
290 
293 
296 
299 
302 
304  double m_length0;
305 
307  double m_length;
308 
309 
310  //-----------------------------------------------------------------------
311  // STATIC MEMBERS - DEFAULT SETTINGS:
312  //-----------------------------------------------------------------------
313 
314 public:
315 
317  static double s_default_kSpringElongation; // [N/m]
318 
320  static double s_default_kSpringFlexion; // [Nm/RAD]
321 
323  static double s_default_kSpringTorsion; // [Nm/RAD]
324 
327 };
328 
329 //---------------------------------------------------------------------------
330 #endif
331 //---------------------------------------------------------------------------
void mulr(const cMatrix3d &a_matrix, cMatrix3d &a_result) const
void crossr(const cVector3d &a_vector, cVector3d &a_result) const
double length() const
cVector3d cSub(const cVector3d &a_vector1, const cVector3d &a_vector2)
void mulr(const double &a_scalar, cVector3d &a_result) const
This class implements a mass node with mass and inertia properties.
Definition: CGELSkeletonNode.h:69
void addForce(chai3d::cVector3d &a_force)
This method adds a force to the node.
Definition: CGELSkeletonNode.h:94
cVector3d cMul(const double &a_value, const cVector3d &a_vector)
cVector3d cNormalize(const cVector3d &a_vector)
chai3d::cVector3d m_pos
Position computed.
Definition: CGELSkeletonNode.h:282
chai3d::cMatrix3d m_rot
Rotation computed.
Definition: CGELSkeletonNode.h:285
cVector3d cCross(const cVector3d &a_vector1, const cVector3d &a_vector2)
double cAngle(const cVector3d &a_vector1, const cVector3d &a_vector2)
void addTorque(chai3d::cVector3d &a_torque)
This method adds a torque to the node.
Definition: CGELSkeletonNode.h:100
Implementation of a skeleton mass node.