D:/CHAI3D/build-2-0-0/2.0.0/win32/modules/GEL/CGELSkeletonLink.h

Go to the documentation of this file.

//===========================================================================
/*
    This file is part of the GEL dynamics engine.
    Copyright (C) 2003-2009 by Francois Conti, Stanford University.
    All rights reserved.

    \author    <http://www.chai3d.org>
    \author    Francois Conti
    \version   2.0.0 $Rev: 251 $
*/
//===========================================================================

//---------------------------------------------------------------------------
#ifndef CGELSkeletonLinkH
#define CGELSkeletonLinkH
//---------------------------------------------------------------------------
#include "chai3d.h"
#include "CGELSkeletonNode.h"
//---------------------------------------------------------------------------

//===========================================================================
//===========================================================================

//===========================================================================
//===========================================================================
class cGELSkeletonLink
{
  public:

    //-----------------------------------------------------------------------
    // CONSTRUCTOR & DESTRUCTOR:
    //-----------------------------------------------------------------------

    cGELSkeletonLink(cGELSkeletonNode* a_node0, cGELSkeletonNode* a_node1);

    ~cGELSkeletonLink();


        //-----------------------------------------------------------------------
    // METHODS:
    //-----------------------------------------------------------------------

    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void render()
    {
        // render link
        glColor4fv( (const float *)&m_color);
        glBegin(GL_LINES);
          glVertex3dv( (const double *)&m_node0->m_pos);
          glVertex3dv( (const double *)&m_node1->m_pos);
        glEnd();
    }


    //-----------------------------------------------------------------------
    //-----------------------------------------------------------------------
    inline void computeForces()
    {
        // update basic parameters of current link
        m_wLink01 = cSub(m_node1->m_pos, m_node0->m_pos);
        m_wLink10 = cMul(-1, m_wLink01);
        m_length = m_wLink01.length();

        m_wzLink01 = cMul((m_node0->m_rot), m_nzLink01);
        m_wzLink10 = cMul((m_node1->m_rot), m_nzLink10);

        //-------------------------------------------------------------
        // ELONGATION:
        //-------------------------------------------------------------
        // if distance too small, no forces are applied
        if (m_length < 0.000001) { return; }

        // elongation compute force
        double f = m_kSpringElongation * (m_length - m_length0);

        // apply force
        if (m_length > 0.000001)
        {
            cVector3d force = cMul(f/m_length, m_wLink01);
            m_node0->addForce(force);
            cVector3d tmpfrc = cMul(-1, force);
            m_node1->addForce(tmpfrc);
        }

        //-------------------------------------------------------------
        // FLEXION:
        //-------------------------------------------------------------
        // compute flexion forces and torques
        cVector3d torqueDir0 = cCross(m_wzLink01, m_wLink01);
        cVector3d torqueDir1 = cCross(m_wzLink10, m_wLink10);
        double angle0 = cAngle(m_wzLink01, m_wLink01);
        double angle1 = cAngle(m_wzLink10, m_wLink10);
        double torqueMag0 = angle0 * m_kSpringFlexion;
        double torqueMag1 = angle1 * m_kSpringFlexion;

        if ((m_length > 0.000001) && (m_enabled))
        {
                        if (torqueMag0 > 0.0001)
            {
                cVector3d torque0 = cMul(torqueMag0, cNormalize(torqueDir0));
                m_node0->addTorque(torque0);
                cVector3d force1 = cMul((torqueMag0/m_length), cNormalize(cCross(m_wLink01, torque0)));
                m_node1->addForce(force1);
                cVector3d tmpfrc = cMul(-1,force1);
                m_node0->addForce(tmpfrc);
            }

            if (torqueMag1 > 0.0001)
            {
                cVector3d torque1 = cMul(torqueMag1, cNormalize(torqueDir1));
                m_node1->addTorque(torque1);
                cVector3d force0 = cMul((torqueMag1/m_length), cNormalize(cCross(m_wLink10, torque1)));
                m_node0->addForce(force0);
                cVector3d tmpfrc = cMul(-1,force0);
                m_node1->addForce(tmpfrc);
            }
        }

        //-----------------------------------------------------------------------
        // TORSION:
        //-----------------------------------------------------------------------

                if (m_enabled)
                {
                        // update frame vectors in world coordinates.
                        m_node0->m_rot.mulr(m_A0, m_wA0);
                        m_node0->m_rot.mulr(m_B0, m_wB0);
                        m_node1->m_rot.mulr(m_A1, m_wA1);

                        // compute torsional angle and torque
                        cVector3d v0, v1;
                        m_wA0.crossr(m_wLink01, v0);
                        m_wA1.crossr(m_wLink01, v1);
                        v0.normalize();
                        v1.normalize();

                        cVector3d torque;
                        v0.crossr(v1, m_torsionAxisAngle);
                        m_torsionAxisAngle.mulr(m_kSpringTorsion, torque);

                        m_node0->addTorque(torque);
                        cVector3d tmptrq = -1.0 * torque;
                        m_node1->addTorque(tmptrq);
                }
    }


        //-----------------------------------------------------------------------
    // MEMBERS - MASS NODES:
    //-----------------------------------------------------------------------

    cGELSkeletonNode *m_node0;

    cGELSkeletonNode *m_node1;


        //-----------------------------------------------------------------------
    // MEMBERS - GRAPHICAL PROPERTIES:
    //-----------------------------------------------------------------------

    cColorf m_color;


        //-----------------------------------------------------------------------
    // MEMBERS - PHYSICAL PROPERTIES:
    //-----------------------------------------------------------------------

    double m_kSpringElongation;

    double m_kSpringFlexion;

    double m_kSpringTorsion;

        bool m_enabled;

    //double m_kDamperElongation;

    //double m_kDamperFlexion;

    //double m_kDamperTorsion;


  public:


        //-----------------------------------------------------------------------
    // MEMBERS:
    //-----------------------------------------------------------------------

    cVector3d m_nzLink01;

    cVector3d m_wzLink01;

    cVector3d m_nzLink10;

    cVector3d m_wzLink10;

    cVector3d m_wLink01;

    cVector3d m_wLink10;

    cVector3d m_torsionAxisAngle;

    cVector3d m_A0;

    cVector3d m_wA0;

    cVector3d m_B0;

    cVector3d m_wB0;

    cVector3d m_A1;

    cVector3d m_wA1;

    double m_length0;

    double m_length;


  public:

        //-----------------------------------------------------------------------
    // MEMBERS - DEFAULT SETTINGS:
    //-----------------------------------------------------------------------
      
    static double default_kSpringElongation;   // [N/m]

    static double default_kSpringFlexion;      // [Nm/RAD]

    static double default_kSpringTorsion;      // [Nm/RAD]

    static cColorf default_color;
};

//---------------------------------------------------------------------------
#endif
//---------------------------------------------------------------------------

---

CHAI3D 2.0.0 documentation
Please address any questions to support@chai3d.org
(C) 2003-2009 - CHAI 3D
All Rights Reserved.