ClosestGroundContactSystem.h

Go to the documentation of this file.

/*  Copyright (C) 2003-2021 Free Electron Organization
    Any use of this software requires a license.  If a valid license
    was not distributed with this file, visit freeelectron.org. */

/** @file */

#ifndef __tire_ClosestGroundContactSystem_h__
#define __tire_ClosestGroundContactSystem_h__

#include "solve/solve.h"

namespace fe
{
namespace ext
{

/// @brief Toy/Test Ground Collision System -- M*N: Ground and Collision Points
class ClosestGroundContactSystem :
    virtual public Stepper,
    public Initialize<ClosestGroundContactSystem>
{
    public:
        ClosestGroundContactSystem(void)
        {
        }
        void    initialize(void) {}
virtual void compile(const t_note_id &a_note_id)
        {
            rg_all = m_rg_dataset;
            m_asGround.bind(rg_all->scope());
            m_asPoint.bind(rg_all->scope());
            m_asContact.bind(rg_all->scope());
            m_asLocator.bind(rg_all->scope());
            m_asGround.filter(m_grounds, rg_all);

            for(RecordGroup::iterator i_rg = rg_all->begin();
                i_rg != rg_all->end(); i_rg++)
            {
                sp<RecordArray> spRA = *i_rg;

                if(!m_asContact.check(spRA)) { continue; }

                if(m_asLocator.check(spRA))
                {
                    for(unsigned int i_r = 0; i_r < spRA->length(); i_r++)
                    {
                        m_xform_contacts.push_back(spRA->getRecord(i_r));
                    }
                }
                else if(m_asPoint.check(spRA))
                {
                    for(unsigned int i_r = 0; i_r < spRA->length(); i_r++)
                    {
                        m_point_contacts.push_back(spRA->getRecord(i_r));
                    }
                }
            }
        }
        void    step(t_moa_real a_dt)
        {
            if(!rg_all->scope().isValid()) { return; }

            for(unsigned int i_ground = 0;i_ground<m_grounds.size();i_ground++)
            {
                Record &r_ground = m_grounds[i_ground];

                for(unsigned int i_contact = 0;
                    i_contact < m_point_contacts.size(); i_contact++)
                {
                    Record &r_contact = m_point_contacts[i_contact];

                    // contact point directly above ground
                    m_asContact.contact(r_contact) =
                        m_asPoint.location(r_contact);
                    m_asContact.contact(r_contact)[2] =
                        m_asGround.height(r_ground);
                    m_asContact.normal(r_contact) = SpatialVector(0,0,1);
                }

                for(unsigned int i_contact = 0;
                    i_contact < m_xform_contacts.size(); i_contact++)
                {
                    Record &r_contact = m_xform_contacts[i_contact];

                    // contact point directly above ground
                    m_asContact.contact(r_contact) =
                        m_asLocator.transform(r_contact).translation();
                    m_asContact.contact(r_contact)[2] =
                        m_asGround.height(r_ground);
                    m_asContact.normal(r_contact) = SpatialVector(0,0,1);
                }
            }
        }
    private:
        sp<RecordGroup> rg_all;
        AsGround m_asGround;
        AsContactLive m_asContact;
        AsLocator m_asLocator;
        AsPoint m_asPoint;
        std::vector<Record> m_grounds;
        std::vector<Record> m_xform_contacts;
        std::vector<Record> m_point_contacts;
};

} /* namespace ext */
} /* namespace fe */

#endif /* __tire_ClosestGroundContactSystem_h__ */