TireContactSystem.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_TireContactSystem_h__
#define __tire_TireContactSystem_h__

#include "solve/solve.h"

namespace fe
{
namespace ext
{

/// MOA apply contact to tire (incl/pitch/radius)
class TireContactSystem :
    virtual public Stepper,
    public Initialize<TireContactSystem>
{
    public:
        TireContactSystem(void)
        {
        }
        void    initialize(void) {}
virtual void compile(const t_note_id &a_note_id)
        {
            rg_all = m_rg_dataset;
            m_asContact.bind(rg_all->scope());
            m_asTireLive.bind(rg_all->scope());

            enforce<AsContact,AsContactLive>(m_rg_dataset);

            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_asTireLive.check(spRA)) { continue; }

                for(unsigned int i_r = 0; i_r < spRA->length(); i_r++)
                {
                    setIdentity(m_asTireLive.patch(spRA->getRecord(i_r)));
                    m_tires.push_back(spRA->getRecord(i_r));
                }
            }
        }
        void    step(t_moa_real a_dt)
        {
            if(!rg_all->scope().isValid()) { return; }

            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));

                t_moa_v3 contact_point_tire = transformVector<3,t_moa_real>
                        (inv_tire_transform, m_asContact.contact(r_tire));


                t_moa_real sign_based_tire_frame = 0.0;
                if(contact_point_tire[2] > 0.0)
                {
                    sign_based_tire_frame = -magnitude(contact_point_tire);
                }
                else
                {
                    sign_based_tire_frame = magnitude(contact_point_tire);
                }

                m_asTireLive.contact_radius(r_tire) = sign_based_tire_frame;

                // ground_N_tire -- normal in tire space
                SpatialVector ground_N_tire =
                    rotateVector<3, t_moa_real>
                        (inv_tire_transform, m_asContact.normal(r_tire));

                t_moa_real over_under = dot(ground_N_tire, contact_point_tire);
                if(over_under < 0.0)
                {
                    m_asTireLive.contact_radius(r_tire) =
                        magnitude(contact_point_tire);
                }
                else
                {
                    m_asTireLive.contact_radius(r_tire) =
                        -magnitude(contact_point_tire);
                }

                SpatialVector result;

                // result -- rotation vector for tilting away from upright

#if 0
                t_moa_real side = dot(ground_N_tire, SpatialVector(0,0,1));
                if(side < 0)
                {
                    m_asTireLive.pitch(r_tire) = pi;
                }
                else
                {
                    m_asTireLive.pitch(r_tire) = 0.0;
                }
#endif

                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; }

                if(side < 0)
                {
                    m_asTireLive.pitch(r_tire) -= asin(m);
                }
                else
                {
                    m_asTireLive.pitch(r_tire) += asin(m);
                }
#endif

                // make a patch frame
                t_moa_xform xform;
                setIdentity(xform);

                // Z is N
                xform.up() = ground_N_tire;

                // side is 0,1,0  (TODO:degenerate/laying on side)
                xform.left() = SpatialVector(0,1,0);

                // cross for X
                cross3(xform.direction(), xform.left(), xform.up());
                normalize(xform.direction());

                // orthonormalize Y
                cross3(xform.left(), xform.up(), xform.direction());
                normalize(xform.left()); // should not be necessary

                m_asTireLive.patch(r_tire) = xform;
            }
        }
    private:
        sp<RecordGroup> rg_all;
        AsTireLive m_asTireLive;
        AsContactLive   m_asContact;
        std::vector<Record> m_tires;
};

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

#endif /* __tire_TireContactSystem_h__ */