GroundCollisionSystem.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_GroundCollisionSystem_h__
#define __tire_GroundCollisionSystem_h__

#include "solve/solve.h"

namespace fe
{
namespace ext
{

/// MOA Toy/Test Ground Collision System -- M*N: Ground and Tires
class GroundCollisionSystem :
    virtual public Stepper,
    public Initialize<GroundCollisionSystem>
{
    public:
        GroundCollisionSystem(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_asTireLive.bind(rg_all->scope());
            m_asGround.filter(m_grounds, rg_all);
            m_asTireLive.filter(m_tires, rg_all);
        }
        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_tire = 0; i_tire < m_tires.size(); i_tire++)
                {
                    Record &r_tire = m_tires[i_tire];

                    if(determinant(m_asTireLive.transform(r_tire)) == 0.0)
                    {
                        continue;
                    }
                    SpatialTransform inv_tire_transform;
                    invert(inv_tire_transform, m_asTireLive.transform(r_tire));

                    // contact point directly above ground
                    t_moa_v3 contact_point =
                        m_asTireLive.transform(r_tire).translation();
                    contact_point[2] = m_asGround.height(r_ground);

                    t_moa_v3 contact_point_tire =
                        transformVector<3, t_moa_real>(inv_tire_transform,
                        contact_point);

                    t_moa_v3 Lt = m_asTireLive.transform(r_tire).translation();
                    m_asTireLive.contact_radius(r_tire) =
                        Lt[2] - m_asGround.height(r_ground);

                    // ground_N_tire -- normal in tire space
                    SpatialVector ground_N_tire =
                        rotateVector<3, t_moa_real>
                            (inv_tire_transform, SpatialVector(0,0,1));

                    SpatialVector result;
                    // result -- rotation vector for tilting away from upright
                    cross3(result,ground_N_tire,SpatialVector(0,0,1));

                    t_moa_real m = result[0];

                    // protect the asin() from floating point slightly out
                    if(m > 1.0) { m = 1.0; }
                    if(m < -1.0) { m = -1.0; }

                    m_asTireLive.inclination(r_tire) = asin(m);

#if 0
                    m = result[1];

                    // protect the asin() from floating point slightly out
                    if(m > 1.0) { m = 1.0; }
                    if(m < -1.0) { m = -1.0; }

                    m_asTireLive.pitch(r_tire) = asin(m);
#endif
                }
            }
        }
    private:
        sp<RecordGroup> rg_all;
        AsGround m_asGround;
        AsTireLive m_asTireLive;
        std::vector<Record> m_grounds;
        std::vector<Record> m_tires;
};

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

#endif /* __tire_GroundCollisionSystem_h__ */